使用模型預(yù)測(cè)控制和PID實(shí)現(xiàn)自動(dòng)駕駛的車道保持

自動(dòng)駕駛的3大核心科技是定位(在哪里)，感知(周圍是啥)以及控制(咋開(kāi)車呢)。通過(guò)車道檢測(cè)，我們可以對(duì)車的行進(jìn)路線進(jìn)行路徑規(guī)劃。本篇文章主要通過(guò)一個(gè)自行車的動(dòng)力學(xué)模型討論車輛的加速、剎車和轉(zhuǎn)向的模型預(yù)測(cè)控制。目的不僅在于盡可能地控制車輛軌跡，同時(shí)也還要盡可能使速度平滑以避免暈車和頻繁的剎車。

模型預(yù)測(cè)控制主要在約束條件下使損失函數(shù)最小。例如，我們想要以100ms的周期調(diào)整轉(zhuǎn)向和速度，在轉(zhuǎn)向角度不能超過(guò)25°的約束下，最小化以規(guī)劃的路徑和實(shí)際路徑之間的誤差。我們通過(guò)傳感器獲取車輛的狀態(tài)，比如速度，而我們的動(dòng)作基于傳感器讀數(shù)以一個(gè)短的周期執(zhí)行(例如1s)。例如，我們順時(shí)針轉(zhuǎn)向20°，然后每100ms周期減小1°。加入這些動(dòng)作可以1秒鐘之后的損失函數(shù)最小，我們將會(huì)采用第一個(gè)動(dòng)作：順時(shí)針轉(zhuǎn)動(dòng)20°，但是卻并不執(zhí)行后續(xù)的動(dòng)作，而是在100ms后，重復(fù)優(yōu)化過(guò)程。100ms后，有了新的讀數(shù)，我們就重新計(jì)算下一個(gè)最優(yōu)動(dòng)作。模型預(yù)測(cè)控制通過(guò)預(yù)測(cè)接下來(lái)一段較長(zhǎng)時(shí)間(1s)的損失函數(shù)，來(lái)計(jì)算選擇出下一個(gè)較短周期(100ms)的最優(yōu)動(dòng)作。相比于短視的貪心算法，模型預(yù)測(cè)控制更加健壯，因此能夠控制得更好。

本篇是自動(dòng)駕駛5篇系列之一，這一系列包括：

自動(dòng)駕駛感知：基于卡爾曼濾波得傳感器數(shù)據(jù)融合

自動(dòng)駕駛感知：擴(kuò)展卡爾曼濾波和無(wú)損卡爾曼濾波

自動(dòng)駕駛定位：基于粒子濾波的定位

自動(dòng)駕駛控制：模型預(yù)測(cè)控制和PID控制

自動(dòng)駕駛路徑尋優(yōu)

雙輪運(yùn)動(dòng)學(xué)模型首先，我們定義一個(gè)模型來(lái)描述我們的車輛。

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

(x, y)是車輛的質(zhì)心，ψ是當(dāng)前車身的角度，v是當(dāng)前車輛的速度，lf是當(dāng)前車輛質(zhì)心到原點(diǎn)的距離，β是速度和車身的角度。在我們的例子中，我們假設(shè)β為零，也就是沒(méi)有側(cè)滑。

在我們的模型中，我們可以通過(guò)控制前輪的轉(zhuǎn)角δf以及車輛的加速度a來(lái)控制車輛軌跡。簡(jiǎn)單起見(jiàn)，我們只考慮前輪驅(qū)動(dòng)的車輛，并且將δf記作δ。

模型預(yù)測(cè)控制的細(xì)節(jié)每個(gè)控制周期，我們都從傳感器讀取數(shù)據(jù)并得到車輛狀態(tài)量：

車輛的位置(x，y)

速度v

車身角度 ψ

轉(zhuǎn)向角(舵角) δ

加速度a

軌跡模型：

我們的道路檢測(cè)系統(tǒng)應(yīng)該能夠?yàn)槲覀円?guī)劃好路線，比如，以接下來(lái)6個(gè)航點(diǎn)的坐標(biāo)的形式。在我們的例子中，我們使用6個(gè)航點(diǎn)去逼近一個(gè)3階多項(xiàng)式函數(shù)。我們用這個(gè)模型去計(jì)算y坐標(biāo)和相對(duì)于x軸的車身角度ψ。

動(dòng)態(tài)模型：

接下來(lái)，我們要?jiǎng)?chuàng)建動(dòng)態(tài)模型利用t時(shí)刻的狀態(tài)去預(yù)測(cè)在t+1拍時(shí)刻的車輛狀態(tài)。利用動(dòng)力學(xué)模型，我們可以輕易地從最新時(shí)刻地采樣推導(dǎo)出下一時(shí)刻的位置，車身角度和速度。

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

損失函數(shù)：

在模型預(yù)測(cè)控制中，我們需要定義損失函數(shù)來(lái)優(yōu)化路徑。如果模型不能保持目標(biāo)速度，那么我們就要懲罰模型。如果可能的話，我們并不想要突然的加減速或者突然的轉(zhuǎn)向。但是既然這些實(shí)際上是不可避免的，我們可以盡可能地抑制加減速和轉(zhuǎn)向地變化率。這減輕了暈車同時(shí)更加省油(也省人民幣)。模型的損失函數(shù)應(yīng)當(dāng)包含：雷鋒網(wǎng)

跟蹤誤差

轉(zhuǎn)向誤差

速度損失函數(shù)項(xiàng)(盡量保持在100英里每小時(shí))

轉(zhuǎn)向損失函數(shù)項(xiàng)(盡量避免轉(zhuǎn)向)

加速度損失函數(shù)項(xiàng)(盡量保持0加速度)

轉(zhuǎn)向變化率(越小越好)

加速度變化率(越小越好)

因?yàn)檫@些目標(biāo)也許會(huì)相互沖突，我們需要給這些損失項(xiàng)定義權(quán)重以體現(xiàn)優(yōu)先級(jí)。損失函數(shù)如下：

總而言之：

我們需要用模型預(yù)測(cè)控制來(lái)尋找最優(yōu)路徑，那么就需要?jiǎng)恿W(xué)模型來(lái)預(yù)測(cè)下一拍的狀態(tài)，以下是動(dòng)力學(xué)模型和系統(tǒng)約束：

優(yōu)化模型預(yù)測(cè)控制我們通過(guò)解決一個(gè)約束條件下優(yōu)化損失函數(shù)的問(wèn)題來(lái)解決了控制問(wèn)題。這些約束條件包括油門和轉(zhuǎn)向的控制。

從道路中檢測(cè)下6個(gè)航點(diǎn)，并且計(jì)算3次插值的來(lái)建立行駛軌跡

從傳感器讀取當(dāng)前速度v, 方向ψ, 轉(zhuǎn)向角δ以及加速度 a

使用傳感器讀取的數(shù)據(jù)和動(dòng)力學(xué)模型計(jì)算出第一個(gè)車輛狀態(tài)

根據(jù)1秒內(nèi)的車輛狀態(tài)響應(yīng)優(yōu)化控制動(dòng)作，控制的周期為100ms，所以1s內(nèi)有10個(gè)周期

模型預(yù)測(cè)控制的兩個(gè)變量(也是控制量)：加速度(油門對(duì)應(yīng)正加速度，剎車對(duì)應(yīng)負(fù)加速度)和轉(zhuǎn)向角

給出加速度和轉(zhuǎn)向角的約束范圍

我們將動(dòng)態(tài)模型計(jì)算9次，得到未來(lái)9個(gè)時(shí)間拍的系統(tǒng)狀態(tài)

給出每個(gè)采樣計(jì)算周期的損失函數(shù)

用1個(gè)優(yōu)化器解算出在約束定義下周期1到周期9的最小總損失(注意，在我們的定義中，時(shí)間周期并不從0開(kāi)始，而是從1開(kāi)始到10)

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

但是，我們延時(shí)100ms后再將控制量給模擬器。這樣能夠模擬現(xiàn)實(shí)世界，畢竟處理計(jì)算(讀取傳感器)和執(zhí)行都需要時(shí)間。

從步驟1開(kāi)始重復(fù)，尋找下一個(gè)最優(yōu)控制量。

可調(diào)性：

在我們的例子中，我們計(jì)算了1秒中內(nèi)的最優(yōu)解，這個(gè)參數(shù)是可以調(diào)節(jié)的。長(zhǎng)時(shí)間窗口的優(yōu)化會(huì)給控制器的動(dòng)作漂亮的曲線，但是也會(huì)積累過(guò)多的誤差。實(shí)際上，如果這個(gè)優(yōu)化時(shí)間窗口太大，汽車反而會(huì)脫離期望軌跡。

對(duì)于那些數(shù)學(xué)大神，以下有一個(gè)介紹自動(dòng)駕駛會(huì)怎樣出錯(cuò)的視頻(原文并沒(méi)有給出->_->)。雷鋒網(wǎng)(公眾號(hào)：雷鋒網(wǎng))

PID

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

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

交叉跟蹤誤差的比例部分

用于平滑運(yùn)動(dòng)過(guò)程的交叉跟蹤誤差微分部分

用于消除穩(wěn)態(tài)干擾的交叉跟蹤誤差積分部分