目標(biāo)識(shí)別

關(guān)于目標(biāo)識(shí)別已經(jīng)有大量的案例和教程被公布。使用Tensorflow API或Matlab/Simulink 計(jì)算機(jī)視覺(jué)工具箱，結(jié)合各種類(lèi)型的傳感器數(shù)據(jù)(如3D激光雷達(dá)云點(diǎn)和/或相機(jī)拍攝的照片)，可以從攝像機(jī)視頻流中識(shí)別出目標(biāo)物體。

當(dāng)然，即便通過(guò)ML/DL技術(shù)可以識(shí)別出目標(biāo)物體，仍然遠(yuǎn)遠(yuǎn)不能滿(mǎn)足一個(gè)簡(jiǎn)單的ADAS功能的開(kāi)發(fā)。自動(dòng)駕駛汽車(chē)首先要借助傳感器數(shù)據(jù)正確地理解現(xiàn)實(shí)環(huán)境，然后還要具備思考、規(guī)劃和反應(yīng)的能力。更具體來(lái)說(shuō)，就是系統(tǒng)需要能夠正確地控制車(chē)輛。

模型預(yù)測(cè)控制

基于簡(jiǎn)單的自行車(chē)模型，可以將運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)控制方程輸入模型預(yù)測(cè)控制（MPC）算法。

在當(dāng)今所有的過(guò)程控制中，MPC只是其中一種控制技術(shù)。PID當(dāng)然是用的最多的控制方法，但由于MPC具有多輸入/輸出的優(yōu)化能力和約束條件，使MPC也超過(guò)了10%的占有率。

MPC是一種基于模型的閉環(huán)優(yōu)化控制策略，大量的預(yù)測(cè)控制權(quán)威性文獻(xiàn)都無(wú)一例外地指出, 預(yù)測(cè)控制最大的吸引力在于它具有顯式處理約束的能力, 這種能力來(lái)自其基于模型對(duì)系統(tǒng)未來(lái)動(dòng)態(tài)行為的預(yù)測(cè), 通過(guò)把約束加到未來(lái)的輸入、輸出或狀態(tài)變量上, 可以把約束顯式表示在一個(gè)在線(xiàn)求解的二次規(guī)劃或非線(xiàn)性規(guī)劃問(wèn)題中。

模型預(yù)測(cè)控制具有控制效果好、魯棒性強(qiáng)等優(yōu)點(diǎn)，可有效地克服過(guò)程的不確定性、非線(xiàn)性和并聯(lián)性，并能方便的處理過(guò)程被控變量和操縱變量中的各種約束。

線(xiàn)性時(shí)不變(LTI)控制系統(tǒng)，連續(xù)狀態(tài)空間模型可以這樣描述。

連續(xù)狀態(tài)空間模型。A、B、C、D是常數(shù)狀態(tài)空間矩陣。x是狀態(tài)向量，y是輸出，u是輸入/控制變量

基于一個(gè)簡(jiǎn)單的自行車(chē)模型，狀態(tài)函數(shù)可以寫(xiě)成：

Vy, dot_Vy用于橫向控制，psi, dot_psi用于轉(zhuǎn)向控制，Vx, dot_Vx用于縱向控制。詳細(xì)信息可以從Matlab文檔頁(yè)面找到。

MPC的主要思路是預(yù)測(cè)工廠輸出的產(chǎn)量，優(yōu)化器會(huì)找到控制輸入的最優(yōu)序列，使工廠的產(chǎn)量盡可能接近設(shè)定值。

如下圖中展示了一個(gè)典型的場(chǎng)景，圖中是一輛行駛在十字路口的汽車(chē)。MPC將考慮到道路圖的曲率，并將道路圖和工廠路徑之間的誤差最小化。MPC的主要優(yōu)點(diǎn)之一就是具有硬約束和軟約束能力的多輸入多輸出，非常適合ADAS函數(shù)中的控制策略。

ACC

以下的示例演示了這樣一個(gè)場(chǎng)景：前方一輛汽車(chē)從右邊進(jìn)入了自車(chē)的車(chē)道，雷達(dá)和相機(jī)傳感器識(shí)別到了前車(chē)，并已確認(rèn)。為了安全起見(jiàn)，自車(chē)必須估算與出前方車(chē)輛的相對(duì)距離w.r.t.，如果距離小于允許的距離，則自車(chē)必須拉開(kāi)距離，并保持安全距離直至完全停車(chē)。直到距離前車(chē)足夠遠(yuǎn)，然后自車(chē)逐漸加速，直到達(dá)到預(yù)期的速度。

ACC傳感器融合

在該Test Bench中，ACC的傳感器融合模塊具有檢測(cè)同一車(chē)道(以及傳感器檢測(cè)范圍內(nèi)的其他車(chē)道)是否有前車(chē)的功能，融合測(cè)驗(yàn)(去除冗余)，將檢測(cè)傳遞給MPC，MPC將會(huì)根據(jù)實(shí)際情況相應(yīng)地減慢或加速自車(chē)。

由下圖可見(jiàn)，視覺(jué)和雷達(dá)識(shí)別的對(duì)象、仿真時(shí)間、自車(chē)的縱向速度和路面曲率為輸入?yún)?shù)。傳感器數(shù)據(jù)融合和前車(chē)跟蹤子模塊，包含由于雷達(dá)噪聲引起的第一次雷達(dá)探測(cè)聚類(lèi)，并將來(lái)自視覺(jué)和雷達(dá)的探測(cè)結(jié)合輸入到多目標(biāo)跟蹤器。使用卡爾曼濾波器精確估計(jì)檢測(cè)的狀態(tài)并融合檢測(cè)，然后，利用確定的軌道和道路信息確定自車(chē)與前車(chē)之間的相對(duì)距離和相對(duì)速度，實(shí)現(xiàn)ACC的功能。

利用MPC算法方案，將時(shí)間間隔(可以是駕駛員的反應(yīng)時(shí)間)、縱向速度和駕駛員設(shè)定的速度與相對(duì)距離、相對(duì)速度一起加入自適應(yīng)巡航控制系統(tǒng)。在這個(gè)測(cè)試臺(tái)上，使用了預(yù)構(gòu)建的ACC控制模塊。也可以構(gòu)建特定于用戶(hù)的MPC模塊。

該ACC模塊的主要功能是跟蹤駕駛員設(shè)定的速度，并通過(guò)調(diào)整自車(chē)的縱向加速度來(lái)保持與前車(chē)的安全距離。該模塊使用了模型預(yù)測(cè)控制（MPC）計(jì)算出最佳控制動(dòng)作，同時(shí)還滿(mǎn)足了安全距離和速度，并約束了一定的加速度。

算法結(jié)構(gòu)細(xì)節(jié)如下圖所示。然后用戶(hù)可以從Matlab中對(duì)原始ACC模塊進(jìn)行相應(yīng)的修改。

到目前為止，ACC的主要控制已經(jīng)基本完成。然而，車(chē)輛在行駛時(shí)，司機(jī)還必須要一直保持在車(chē)道上。因此，車(chē)道跟蹤功能（也就是轉(zhuǎn)向控制）也需要考慮在內(nèi)。

隨著MPC縱向加速度的調(diào)節(jié)，Simulink塊中必須要輸入道路(地圖)信息。在本次的測(cè)試用例中，道路幾何簡(jiǎn)單地通過(guò)常曲率1/R來(lái)描述，并已創(chuàng)建于Matlab的工作空間中，可以直接從子系統(tǒng)中使用。

使用MPC調(diào)節(jié)縱向加速度和曲率的道路更新的位置和偏航角的自車(chē)，轉(zhuǎn)向控制采用PID控制。

MPC能夠?qū)ψ攒?chē)的加速度進(jìn)行調(diào)節(jié)，結(jié)合采用比例積分微分(PID)控制方案，將可以實(shí)現(xiàn)轉(zhuǎn)向控制。

基于自行車(chē)模型，可以模擬出自車(chē)的位置和偏航角。

到這里，我們已經(jīng)具備了運(yùn)行ACC模擬的所有必要條件。單擊run按鈕，可以查看結(jié)果，如下圖所示（只顯示一幀）。

整個(gè)仿真結(jié)果如下：

當(dāng)一輛他車(chē)低速車(chē)行駛進(jìn)入速度較快的自車(chē)的車(chē)道時(shí)，只要傳感器檢測(cè)到低速行駛的前車(chē)，在MPC控制的幫助下，自車(chē)會(huì)先減速以保持安全距離。當(dāng)前車(chē)離開(kāi)同一車(chē)道時(shí)，自車(chē)再次加速，加速直到駕駛員設(shè)定的速度。自車(chē)的速度和駕駛員設(shè)定的速度如下圖所示，并顯示了自車(chē)的相對(duì)距離和加速度曲線(xiàn)。

最后這點(diǎn)很重要，ADAS必須使用C或C++部署到特定的ECU上，而Matlab提供了代碼生成器工具箱，可以輕松實(shí)現(xiàn)這一點(diǎn)。如果需要添加或進(jìn)一步修改C算法，則可以基于生成的C/C++代碼繼續(xù)編寫(xiě)。

以上回顧了利用Matlab/Simulink實(shí)現(xiàn)ACC的Test bench。對(duì)于更復(fù)雜的或不同的駕駛場(chǎng)景，各位朋友們可以使用本文描述的類(lèi)似方案來(lái)進(jìn)行進(jìn)一步的功能開(kāi)發(fā)。