代碼目錄如下：

運(yùn)行案例

啟動(dòng)底盤和雷達(dá)

SSH連接OriginBot成功后，在終端中輸入如下指令，啟動(dòng)機(jī)器人底盤和激光雷達(dá)：

自動(dòng)軌跡跟蹤

選擇Pure Pursuit：

選擇 MPC ：

同時(shí)，打開rviz后可以在界面中看到，有如下軌跡顯示

原理淺析

PurePuesuit 和 MPC 算法是自動(dòng)駕駛中常用的控制算法，目的是為了讓規(guī)劃處的軌跡點(diǎn)更加平滑以及滿足車輛本身的運(yùn)動(dòng)學(xué)軌跡。在Autoware和Apollo 中，這兩種算法都是有其實(shí)現(xiàn)方式的?？蓞⒁姡?/span>

Apollo：

Autoware：

概述

在具體聊到兩種算法實(shí)現(xiàn)前，需要先了解到，現(xiàn)在主流的兩種軌跡跟蹤算法分為基于幾何的跟蹤以及基于模型的跟蹤方法。

在此次實(shí)現(xiàn)上基本可以視為已知坐標(biāo)信息，包括(x,y,yaw)和曲率Kappa,計(jì)算出控制量轉(zhuǎn)向角，也即橫向運(yùn)動(dòng)量，使得小車可以在軌跡內(nèi)平穩(wěn)運(yùn)行。

以MPC為例，圖示如下：

Pure Pursuit

純跟蹤算法提出“預(yù)瞄距離”的概念，根據(jù)預(yù)瞄距離尋找目標(biāo)軌跡中符合條件的目標(biāo)路徑點(diǎn)，判斷邏輯就是尋找目標(biāo)軌跡上哪個(gè)點(diǎn)和當(dāng)前車輛位置的相對(duì)距離等于預(yù)瞄距離，則該點(diǎn)就是當(dāng)前時(shí)刻的目標(biāo)點(diǎn)?？刂颇繕?biāo)則是計(jì)算多大的前輪偏角，可以使當(dāng)前小車位置運(yùn)動(dòng)到目標(biāo)位置。在這個(gè)基礎(chǔ)上，不妨簡(jiǎn)單看一下其中的核心點(diǎn)，預(yù)瞄距離是什么。

簡(jiǎn)單來說，預(yù)瞄距離好比人開車時(shí)后找一個(gè)跟蹤參考點(diǎn)，比如在行駛在直道時(shí)，我們會(huì)選擇較高的速度，并且習(xí)慣考慮前方較遠(yuǎn)的點(diǎn)作為跟蹤參考點(diǎn)；在行駛在彎道時(shí)，就會(huì)選擇減速，習(xí)慣選擇較近的點(diǎn)作為跟蹤參考點(diǎn)。所以這個(gè)值是我們可以自己設(shè)置的一個(gè)值。這個(gè)值的設(shè)置也會(huì)極大影響小車的運(yùn)動(dòng)。比如在此次示例的代碼中在findCloestindex函數(shù)內(nèi)，我們根據(jù)預(yù)瞄距離找到了一個(gè)最近的點(diǎn)

獲取到這個(gè)點(diǎn)之后，在根據(jù)小車本身的模型限制計(jì)算出轉(zhuǎn)角：

MPC的求解相對(duì)來說更為復(fù)雜。直接亮出代碼框架：

1、獲取車體參數(shù)。

2、選取狀態(tài)量和控制量。

3、對(duì)AB矩陣做離散化，Apollo中增加了擾動(dòng)矩陣。

4、設(shè)定QR矩陣。

5、設(shè)定預(yù)測(cè)步數(shù)，并對(duì)ABQR做增廣。

6、設(shè)定相關(guān)參數(shù)的約束并獲取各傳感器的值。

7、求解控制量u，最后拿到u[0].

更為具體的邏輯可以參考此鏈接：https://blog.csdn.net/u013914471/article/details/83824490