本文接近25000字

從理論到實踐，全部零基礎(chǔ)開始，一步一步完成自動駕駛車輛控制基礎(chǔ)

感謝：忠厚老實的老王

下面是他的主頁：忠厚老實的老王的個人空間_嗶哩嗶哩_bilibili

目錄

第一講? 緒論

第二講? 三個坐標系與運動學(xué)方程

一、控制原理

二、三個坐標系

三、左手系與右手系

四、自行車模型的運動學(xué)方程

第三講 輪胎側(cè)偏與車輛動力學(xué)方程

一、動力學(xué)方程與Frenet坐標系解耦

二、輪胎的側(cè)偏特性

三、考慮輪胎側(cè)偏特性的自行車模型

第四講 坐標變換與橫向誤差微分方程

第五講 連續(xù)方程的離散化與離散LQR原理

一、離散化模型

?二、求解lqr

三、黎卡緹方程

四、總結(jié)

第六講 前饋控制與航向誤差

一、前饋控制

第七講 離散規(guī)劃軌跡的誤差計算

一、離散的軌跡點的誤差計算

第八講

一、橫向控制算法與流程圖

（1）算法輸入輸出過程

（2）流程圖

?（3）具體算法

?（4）預(yù)測模塊

二、CarSim與Matlab聯(lián)合仿真基礎(chǔ)

三、代碼與模型

第九講? 縱向控制開篇

一、縱向控制邏輯

二、發(fā)動機與變速器

三、CarSim基礎(chǔ)設(shè)置

第十講 油門剎車標定表的制作

一、理論部分

二、油門標定表制作

三、剎車標定表制作

第十一講 縱向雙PID控制

一、前置準備

二、油門和剎車標定表制作

三、縱向速度PID控制仿真

第十二講? 橫縱向綜合控制完結(jié)

一、規(guī)劃接口

二、代碼實踐部分

三、轉(zhuǎn)向不足

第一講? 緒論

請用謹慎，批判 ，懷疑的態(tài)度去研究自動駕駛的相關(guān)資料文獻

新技術(shù)往往帶有模糊和爭議，自動駕駛是新技術(shù)，也不能避免

工具：google，Github

軟件：matlab（2018a）、Carsim 2016、ROS、Linux

書：《無人駕駛車輛模型預(yù)測控制》龔建偉（第二版）（第一版很多錯誤）?

《車輛動力學(xué)及控制》=>《Vehicle Dynamics and Control》R.R. （百度Apollo）

自動駕駛技術(shù)：感知->決策->路徑規(guī)劃->控制

感知與決策難，控制難在調(diào)參，主要算法為PID，LQR，MPC

芯片計算速度（臺式機性能的芯片）車載芯片工作環(huán)境極為惡劣

第二講? 三個坐標系與運動學(xué)方程

控制的前提是路徑規(guī)劃，默認已經(jīng)有路徑規(guī)劃的儲備知識

一、控制原理

油門/剎車 -> 力 -> 加速度?-> 速度?-> 位置（縱向控制）

方向盤?-> 前輪轉(zhuǎn)角?-> 橫向位移/航向角（橫向控制）

二、三個坐標系

?Frenet坐標系優(yōu)點是：可以把縱向控制和橫向控制解耦，減少計算量

車身坐標系與自然坐標系轉(zhuǎn)換會難一些

三、左手系與右手系

一般數(shù)學(xué)，物理學(xué)是右手系。計算機圖形，視覺為左手系

四、自行車模型的運動學(xué)方程

自行車模型將車簡化為前后兩個輪子和一個軸，假設(shè)車輪輪距為0，

• ?求質(zhì)心速度：瞬心法

假設(shè)前輪速度v'，后輪速度為v，利用瞬心法求出質(zhì)心速度vc

• ?橫擺角與航向角

?注意這里的前輪轉(zhuǎn)角和后輪轉(zhuǎn)角會由于輪胎的剛度而并不與方向盤轉(zhuǎn)角一一對應(yīng)，這里假設(shè)低速不考慮輪胎變形的自行車模型，所以前輪轉(zhuǎn)角可以控制航向角

• 縱向車速與橫向車速

?下面建立微分方程：即前輪轉(zhuǎn)角如何影響到側(cè)向位移與航向角

①根據(jù)幾何關(guān)系建立運動學(xué)模型；②根據(jù)牛頓力學(xué)建立動力學(xué)模型

動力學(xué)模型更加復(fù)雜，考慮輪胎變形，參數(shù)很難準確。

運動學(xué)模型簡單，所以只適用于低速轉(zhuǎn)彎半徑大的情況。

• 運動學(xué)模型

首先角度推導(dǎo)

利用速度投影得到幾何關(guān)系：

此時并未體現(xiàn)前輪轉(zhuǎn)角對于的影響，要進行進一步推導(dǎo)

設(shè)前輪到質(zhì)心距離a，后輪到質(zhì)心距離b，根據(jù)正弦定理

?展開后得到

?由于質(zhì)心位置會變，所以a和b是變量，需要用一些不變的量來表示

使用（a+b）/R來表示，因為a+b=L為軸距不會變

這里由于質(zhì)心側(cè)偏角≈0，所以cosβ≈1

帶入上面第三個式子，由此得到運動學(xué)方程：

?在低速條件下，認為車不會發(fā)生側(cè)向滑動（漂移），vy≈0，這個時候質(zhì)心側(cè)偏角為0

一般后輪不轉(zhuǎn)向，在低速條件下，認為=0，所以有下面這個運動學(xué)方程

?由于beta=0，所以橫擺角約等于航向角

?其中為橫擺角，v是質(zhì)心速度

第三講 輪胎側(cè)偏與車輛動力學(xué)方程

運動學(xué)方程：簡化過多，高速情況不適用

?動力學(xué)方程：考慮輪胎特性

一、動力學(xué)方程與Frenet坐標系解耦

當(dāng)選取Frenet坐標系時，可以將縱向控制與橫向控制解耦

縱向控制只需要管油門和剎車，不需要管方向盤，即與橫向控制解耦

當(dāng)縱向穩(wěn)定時，橫向控制只與方向盤轉(zhuǎn)角有關(guān)

而運動學(xué)方程無法解耦，這就是為什么要使用動力學(xué)方程的原因

動力學(xué)+Frenet坐標系可以解耦

縱向位移s，s兩階導(dǎo)數(shù)是油門剎車決定的縱向加速度

橫向位移d與方向盤轉(zhuǎn)角的關(guān)系，這個關(guān)系汽車理論中給了，但是是在車身坐標系下面，所以如何把這個公式轉(zhuǎn)換到Frenet坐標系是關(guān)鍵

下面的橫向位移都用y來表示

二、輪胎的側(cè)偏特性

車輛輪胎受力后會變成圓臺一樣的形狀，只能走圓弧，

為什么輪胎變形是一個梯形呢，因為梯形繞輪胎轉(zhuǎn)軸轉(zhuǎn)180還是原來的樣子，而平行四邊形不是了

側(cè)偏會導(dǎo)致輪胎實際速度與期望速度方向有一個夾角，稱之為側(cè)偏角

側(cè)偏角與受的側(cè)向力呈正比

側(cè)偏角定義：，其中C為側(cè)偏剛度，側(cè)偏剛度*側(cè)偏角= 側(cè)向力

注意：側(cè)偏剛度一定為負數(shù)，負的側(cè)偏力=>正的側(cè)偏角

此外，側(cè)向力一般用Fy來表示

三、考慮輪胎側(cè)偏特性的自行車模型

二自由度模型可以在兩個自由度的前提下，模型表示的比較準

注意都是負的，根據(jù)受力分析得到：（注意第一個公式是may，不是ay）

?假設(shè)比較小，有，把側(cè)偏公式帶入得到

驗證：L為軸距，約等于3m；R為轉(zhuǎn)彎半徑，一般8-12m；

即

下面給出與y的關(guān)系以及的具體表達式

?與y的關(guān)系直接給出：

?注意這里后面還多了一項，因為這個公式是在車身坐標系下，要帶上慣性力

加速度是個矢量，等于xy兩個方向的速度乘單位矢量和，

?在直角坐標系下，單位矢量ex和ey是常矢量

在車身坐標系下，ex和ey不是常矢量，所以其導(dǎo)數(shù)不等于0，因此多出來了一項

下面推導(dǎo)，

根據(jù)剛體的速度合成與分解，把vr移動到質(zhì)心處，連接vr與v的線一定與車的軸線垂直，并且大小等于

下面放大看

?藍色線的長度，豎著的藍線為，進而推出

?為了保證是負數(shù)，一半phib比較大，vy比較小，所以可以交換一下位置，改變正負號

下面畫出和，圖中綠色角是，上面的角是

注意這里是前輪速度與車身x軸的夾角，是前輪轉(zhuǎn)角?？梢酝ㄟ^公式推出，這樣就得到表達式。

我們把局部圖放大來看，藍色的角為上面綠色的角

對取tan得到，根據(jù)幾何關(guān)系進而得到：，即，然后跟上面一樣需要加一個負號保證\alpha_{f}為負數(shù)，就得到了下式

?得到帶入上面公式得到（這里，下面公式寫錯）

?整理得到

?即可得到狀態(tài)空間方程

第四講 坐標變換與橫向誤差微分方程

要控制的目標就是：讓汽車當(dāng)前狀態(tài)與規(guī)劃的狀態(tài)接近

規(guī)劃給出：車位置（），速度、航向角、加速度?，則可以計算出五個誤差

定義：真實的-規(guī)劃的 = 誤差，得到誤差的表達式?

控制目標是

?定義一個代價函數(shù)J= a*誤差平方+b*u的平方，可以改寫成向量形式

根據(jù)投影法定義5個誤差，將速度v在自然坐標系下投影到規(guī)劃的位置的切向與法向得到和d，投影在上面的速度為，車到投影的距離設(shè)為

?下面定義誤差：橫向誤差自然為d，航向誤差為，投影的速度大小為，速度誤差為（這里是大小，不含方向，因為航向角控制了方向），速度誤差屬于縱向控制，暫時先不管

注意：這里的\tau和n和車身坐標系的x和y不是一個坐標系，因為v是質(zhì)心速度，質(zhì)心速度和絕對坐標系x軸的夾角為航向角\theta，而車身坐標系是以橫擺角為方向，x軸是沿著橫擺角的方向，y軸垂直橫擺角方向，所以和n不是車身坐標系的x和y。（個人理解，兩者差了一個質(zhì)心側(cè)偏角）

d為橫向誤差，和規(guī)劃位矢xr與真實位矢x組成三角形，

根據(jù)矢量關(guān)系寫出表達式，（這里d是一個數(shù)，不是向量，所以需要乘以方向向量nr）

算出d的表達式還不夠，因為我們的狀態(tài)空間方程式以為未知量，然而d和和沒聯(lián)系，為了聯(lián)系起來，對d求導(dǎo)得

其中

這里注意nr并不是一個固定的常矢量，其方向會隨著規(guī)劃的位置而改變，和曲率有關(guān)

推導(dǎo)，假設(shè)矢量r經(jīng)過dt時間到達r+dr，藍色的為差值位矢dr，紅色的弧線為ds

代入d的求導(dǎo)得到（注意這里前面的括號里，?和?垂直，所以點乘=0可以消掉）

最后一項，，這個沒啥好說的，但是是啥呢？

這里就要用到Frenet公式求

帶入得到

帶入到上面公式得到，（?和?垂直，所以點乘=0可以消掉后面一項）

?為了求和之間的夾角的關(guān)系，畫出圖，那么和之間的夾角可以通過幾何關(guān)系得到

?帶入后得到

接下來計算

對求導(dǎo)得到

再把上面推導(dǎo)的結(jié)果以及Frenet公式帶入得到

?兩邊同時得到：

和之間的角度實際上就是

推導(dǎo)到這里，我們得到兩個公式：

這兩個公式非常重要，是一切工作的起點

還得把它與聯(lián)系起來，帶入航向角得到：?

注意上式中，自行車模型中vy喝vx都可以用v和質(zhì)心側(cè)偏角表示

d為縱向誤差，令，（注意：其中不是航向誤差，應(yīng)該是，少了質(zhì)心側(cè)偏角，但是一般比較小，在可以忽略，但這個比較重要后面不能忽略）

求出vy和ephi的一階二階導(dǎo)數(shù)

一般道路平緩，所以忽略的二階導(dǎo)數(shù)

?帶入二自由度動力學(xué)方程，再化簡得到ed兩點（這里是由于要消掉vx和vy）

同理帶入，得到\e_{\varphi}兩點

?這兩個方程就是誤差的微分方程，下面進行簡化?

湊成下列控制方程?，由此得到誤差線性微分方程

遺留的三個問題

（1）航向誤差問題：缺少質(zhì)心側(cè)偏角

（2）如何計算方程中最后一項：

（3）去掉最后一項小尾巴可以用LQR計算，加上后怎么控制

第五講 連續(xù)方程的離散化與離散LQR原理

先暫時忽略這一項，公式轉(zhuǎn)換為

Matlab有現(xiàn)成的包：lqr(A,b,Q,R)和dlqr(A,B,Q,R) (離散化LQR)可以算出u

主要應(yīng)用為dlqr，連續(xù)lqr很難

dlqr原理只涉及到拉格朗日乘子法

拉格朗日乘子法：求f(x,y)在約束g(x,y)=0條件下的極值：

dlqr問題：

?求

?這個極小值一定是最小值

所以將dlqr問題分解成倆問題

一個是如何離散化 ，另一個是如何求解優(yōu)化問題

一、離散化模型

離散化：可以使用向前歐拉法或者中點歐拉法，apollo是兩者的混合

先積分消掉x·，再使用中值定理進行近似

向前歐拉法、向后和中點歐拉法則是如何替代?

可以將x(t+dt)用x(t)表示出來

對于我們這個問題就可以先積分再用中值定理

然后再將和代替，這里使用向前歐拉，因為u(t+dt)不知道

?化簡得到

dt為采樣周期，完成離散化

?二、求解lqr

下面推導(dǎo)ldqr的解法

先讓J從0計算到n，然后再讓n趨于無窮。這里拆分成0到n-1和第n項。(這一步我的理解是，因為要推導(dǎo)出第n和n-1步之間的遞推關(guān)系，所以要這么分解)

?寫出約束

?約束剛好完全覆蓋自變量，如果把第n個約束添加進去，會多加一項x_{n+1}

如果n趨于無窮，有沒有n+1這項無所謂

所以可以使用拉格朗日乘子法，把J寫成

?下面對其化簡

下面要對J進行求導(dǎo)，所以先給出向量導(dǎo)數(shù)的定義 ：

這里的?

分別對自變量求導(dǎo)得到

再把剛才化簡得Hk求偏導(dǎo)帶入上面的式子中

得到以下式子（注意這里筆誤：這里③式k是從0開始），一共是3n-2個式子

?下面基于上式求出遞推式

?將⑧與④進行對比

為了求出與的關(guān)系

得到Pn=Q以及Pk-1的表達式：

??基于這兩個式子，可以以Pn=Q為初始值，一直從k=n推導(dǎo)到k=0

?也就是說我們知道了與的關(guān)系，下面再關(guān)注②式，可以將由表示出來

式子中的R，B，A均為已知量，P是黎卡緹方程算出來的

注意這里的xk，一般可以用誤差來表示，根據(jù)上面對狀態(tài)空間方程的定義有xk=err

err可以通過定位和規(guī)劃得到

所以uk可以由已知量算出

此外注意，這里的輸入uk是車的前輪轉(zhuǎn)角，只有一個數(shù)，因此B是一個4*1的向量

三、黎卡緹方程

注意：對于黎卡緹方程，如果n區(qū)域無窮大，那么黎卡緹方程要迭代無數(shù)次，但是實際情況是Pk迭代幾十次后就會收斂，即從Pk一直到P0均相等，這個性質(zhì)非常好，可以幫助我們處理無窮時間

所以

解黎卡緹方程的步驟為先給P取初值，然后迭代求出P，然后再根據(jù)P求出u

課本上面的黎卡緹方程為

和我們這里其實一樣，只是方程形式寫的不一樣，推導(dǎo)一下

?我們上面推導(dǎo)的黎卡緹方程為4*4矩陣求逆，課本給出的這個是1*1求逆，即求倒數(shù)。

矩陣求逆是基于數(shù)值的方法，所以1*1求逆精度會更高，所以推薦使用下面這種。

四、總結(jié)

第六講 前饋控制與航向誤差

的框圖表示

如果u = -kx，即相當(dāng)于加上一個反饋，如圖所示

?容易出現(xiàn)代數(shù)環(huán)問題，輸入影響輸出，輸出影響輸入，這里先不管，后面可以用matlab加延時的方法解決

一、前饋控制

在上面的框圖內(nèi)再加一個輸入量，變成前饋控制?

因為我們要解決第四講的遺留問題，即如何處理這個小尾巴

如果只用LQR，

無論k取何值，和不可能同時為0，即不可能永遠為0，因為，不是該微分方程的解

所以要引入前饋控制：，其中-kx是由LQR計算出的反饋控制，為前饋控制

前饋控制的引入是為了消除穩(wěn)態(tài)誤差

LQR最終會導(dǎo)致，，此時的穩(wěn)態(tài)誤差可以計算得到：

引入前饋控制后，有?

穩(wěn)定后的

目的：選取合適的，使得盡可能為零

上面已經(jīng)推導(dǎo)出了A，B，C，注意這里的

?把ABC帶入，用軟件Mathmatica計算得到err的表達式

可以看出每項都有這一項，此外注意

對的影響

?先算反饋K，再算前饋

無論前饋反饋取何值，

下面對進行化簡，首先利用幾何關(guān)系求出

?求曲率

利用曲率k求出與s的關(guān)系，這邊做了很多假設(shè)

?帶入后得到與vx的關(guān)系

?然后求出ay和vx的關(guān)系，

再帶入上式把vx2消掉，得到

其中，may為總側(cè)向力，cr為后輪側(cè)偏剛度

將車輛等效成前后兩部分，分別計算出前后質(zhì)量

根據(jù)后輪側(cè)向力與ay的關(guān)系，以及等效關(guān)系，將其進一步化簡

?根據(jù)幾何關(guān)系

?得到的剛好等于，

??雖然不是航向誤差，但是其穩(wěn)態(tài)誤差為

雖然不可能通過，K去調(diào)節(jié)，但是我們不用理會。

因為最終目的是，即，而的穩(wěn)態(tài)誤差恰好為，所以不用管。

?至此，得到u的表達式

?其中K是通過lqr計算得到，通過前饋控制得到

第七講 離散規(guī)劃軌跡的誤差計算

基于前六講已經(jīng)算出u的表達式

誤差的計算在第四講

其中：

?只要知道

?就可以把誤差計算出來

?若規(guī)劃的曲線是連續(xù)的，可能會導(dǎo)致投影不唯一

若A與A'的連線與A'的切線垂直，則A'為A的投影?

若曲線是連續(xù)的，不僅僅是投影麻煩，而且要處理多值問題，因此要離散

一、離散的軌跡點的誤差計算

將軌跡離散成一些點，每個點都有四個信息

如何計算基于離散的誤差呢？

方法如下：

①找到離散軌跡規(guī)劃點中與真實位置（x,y）最近的點

?這個點在Apollo里面稱之為match-point（匹配點）

②匹配點代替投影點的前提：規(guī)劃的點很密

?現(xiàn)實情況是規(guī)劃不能很密，否則計算量過大，所以需要通過匹配點近似算出投影點

?假設(shè)：從匹配到投影點的曲率k不變。即認為匹配點和投影點的軌跡近似用圓弧代替

匹配點的切向向量和法向向量

匹配點與車實際點的向量：

?③求出ed（有正負：左為正，右為負）

?④求出es：匹配點與投影點的弧長（es有正負）

?正代表投影在匹配點前面，負代表在匹配點后面

?⑤（apollo）

⑥算出誤差

?這樣就可以算出離散的err，再使用離散LQR計算出K，進而計算出

第八講

一、橫向控制算法與流程圖

（1）算法輸入輸出過程

首先要搞明白算法，才能寫代碼

?注意這里第四步為：，不是x

（2）流程圖

?（3）具體算法

?①A，B計算模塊

up勘誤:這里矩陣第四行第二列第三列寫錯了,第五講開頭那個矩陣才是對的

注意要考慮當(dāng)= 0時的奇異性?

②LQR模塊

因為A，B只與整車參數(shù)和有關(guān)，整車參數(shù)近似認為不變

情況1：如果加速度非常大，慣性力就比較大，這個時候前輪垂向力減少很多，后輪加大，側(cè)偏剛度改變。此時仍然可以使用自行車模型，只是側(cè)偏剛度改變。

情況2：急速過彎時，左右輪不對稱，導(dǎo)致自行車模型不適用，所以在路徑規(guī)劃的時候要極力避免急速過彎的情況

A，B只與整車參數(shù)和有關(guān)，或者?

每個都有唯一的一個K與之對應(yīng)

所以可以離線算出與K的對應(yīng)表，實際應(yīng)用中不需求解Riccati方程，直接查表

優(yōu)點：速度大大加快

缺點：耗費儲存空間（空間換時間）?

個人理解：針對每個vx，可以求出對應(yīng)的AB，對于每個AB都可以迭代算出P，然后算出對應(yīng)的K

MATLAB自帶的模塊，只需要輸入A，B，Q，R即可得到K

③計算模塊

首先要有離散的規(guī)劃點

遍歷找到距離當(dāng)前車的位置最近的規(guī)劃點，該點的序列（index）記為dmin

寫出匹配點的切向向量與法向向量

寫出誤差距離向量(個人理解：這里的x_dmin就是上面的xm，即投影點的坐標，d_err為匹配點與車實際位置之間的位矢)

?算出誤差ed和es（個人理解：ed是投影點和實際位置之間的位矢，es是投影點與匹配點之間的位矢）

算出（個人理解：這里的是匹配點的曲率）

?算出其他誤差和變量

?輸出

④前饋控制計算模塊

⑤最終控制計算模塊

?（4）預(yù)測模塊

車有慣性，所以要提前控制，要增加一個預(yù)測模塊

舉例：如下圖，假設(shè)當(dāng)前位置不在路徑上面，但是速度朝向路徑，如果是人在開車，他不會打方向盤改變方向，但是算法為了減小誤差，它會打方向盤為了減少誤差

這個例子也是一樣，如果當(dāng)前點在路徑上，算法不會打方向盤，但是人知道要轉(zhuǎn)彎

算法控制具有滯后性，所以要加上預(yù)測模塊，讓算法有預(yù)見性

解決辦法：使用預(yù)測點代替真實點去計算誤差

?預(yù)測算法寫法：

首先有個預(yù)測時間，記為ts，可以使用速度×?xí)r間得到位置

?根據(jù)運動計算出預(yù)測點的全部信息

至此介紹完畢算法流程圖，下面搭建仿真平臺

二、CarSim與Matlab聯(lián)合仿真基礎(chǔ)

CarSim這里使用2019版本，具體下載和安裝可參考下面這個視頻

最新！Carsim2019詳細安裝教程【附安裝包】_嗶哩嗶哩_bilibili

?（1）CarSim于Simulink基礎(chǔ)設(shè)置

首先要知道兩個重要路徑

CarSim默認數(shù)據(jù)庫路徑：C:\Users\Public\Documents\CarSim2019.0_Data

CarSim安裝路徑：E:\CarSim2019.0_Prog

打開CarSim軟件，需要選擇數(shù)據(jù)庫路徑

?點擊continue進入

注意這個時候是灰色的不可選的狀態(tài)，點擊Lock就可以解鎖，進行選擇

?點擊這里選擇車輛，ABCDE級車，每個車的慣量，尺寸等參數(shù)都不一樣

整車尺寸參數(shù)

進入輪胎界面，可以看到Fy：側(cè)偏曲線：側(cè)向力與側(cè)偏角的關(guān)系曲線，

由于做控制，不允許大側(cè)偏角，容易失穩(wěn)，只考慮小側(cè)偏角下的曲線，此時斜率為側(cè)偏剛度

?model選擇Simulink

?點擊這個地方，新建一個數(shù)據(jù)庫

新建數(shù)據(jù)庫，起名叫a

?然后需要填入simulink模型，這里沒有，所以新建一個，打開Matlab

?打開MATLAB，選擇Simulink

?創(chuàng)建好空模型，然后直接Save as，保存到數(shù)據(jù)庫路徑，設(shè)置文件名為a，這里可以和CarSim的不一樣

?將剛才兩個重要路徑都添加進MATLAB路徑

?（2）輸入輸出設(shè)置

設(shè)置輸入

?新建數(shù)據(jù)庫并起名字ain‘

同樣把輸出aout也新建好

?點擊ain，進入設(shè)置，選擇Baseline

選擇Select by units

將油門，前輪后輪轉(zhuǎn)角都加入，完成設(shè)置

注意：L1是左前，L2是左后，R1是右前，R2是右后

?同理設(shè)置輸出，為如下幾個變量

?接下來回到主界面，然后選擇SHow，Adcanced，再加上這句話：?opt_steer_ext(1) 4

因為目前只能通過方向盤轉(zhuǎn)角來控制，而我們是通過前輪轉(zhuǎn)角控制，加上之后就是前輪轉(zhuǎn)角控制

??Procedure里面可以設(shè)置仿真時長和距離

這里設(shè)置為10s和21000m

?一切準備就緒，點擊Send to simulink

打開Simulink后，有兩種方法導(dǎo)入SF函數(shù)，方法1是和老王一樣，這種在2016可以用，點擊選擇文件，進入安裝路徑，選擇Solver_SF.mdl打開

方法2是CarSim2019可以直接在libray拉進來

不管是那種方法，都需要把SF函數(shù)這個里面填入名字和數(shù)據(jù)庫路徑下的sim文件名字相同

然后加一個5輸入mux，，分別是輸入油門設(shè)這位0.5，前后輪轉(zhuǎn)角設(shè)置為0。后面再加一個demux，前兩個接給示波器看下位置變化

點擊仿真Run

再點擊示波器

?回到CarSim，點擊Video就可以看到仿真視頻

其結(jié)果應(yīng)該是走一條直線（因為并未給前后輪轉(zhuǎn)角），共花費10s

?Simulink將前輪轉(zhuǎn)角改成30，看仿真結(jié)果

結(jié)果為原地畫圓

三、代碼與模型

代碼下載地址：老王github上VincentWong3/automated-driving-control (github.com)

嫌麻煩的話可以在這里下載

automated-driving-control-main.zip - 藍奏云

在寫代碼之前，先要明確算法流程

我們需要的是整車參數(shù)，車輛狀態(tài)以及規(guī)劃

整車參數(shù)可以在CarSim里面

?注意這里的質(zhì)量1270是簧上質(zhì)量，還需要加上懸架質(zhì)量71，總質(zhì)量是1412

?此外需要注意的是側(cè)偏剛度的單位，以及加上負號，由于是自行車模型還要乘以2

?把空氣動力學(xué)關(guān)掉，暫時忽略風(fēng)阻的影響

?由于需要用到車輛狀態(tài)，所以需要在CarSim中輸出狀態(tài)

（1）路徑規(guī)劃信息生成

由于需要規(guī)劃的路徑，所以這里需要一個規(guī)劃函數(shù)給出信息，這里對應(yīng)的是routing_planning.m文件，里面有兩個函數(shù)，一個是直線，一個是圓弧

straight函數(shù)：輸入起點和終點坐標以及初始角和離散點數(shù)量，生成這樣一系列點

function[xr,yr,thetar,kr]=straight(init_coord,end_coord,init_angle,count)
delta_x=(end_coord(1)-init_coord(1))/(count-1);
delta_y=(end_coord(2)-init_coord(2))/(count-1);
for i=1:count
    xr(i)=init_coord(1)+delta_x*i;
    yr(i)=init_coord(2)+delta_y*i;
    thetar(i)=init_angle;
    kr(i)=0;
end      
end

用法舉例：

[xr,yr,~,kr]=straight([0,0],[50,50],pi/4,10);
scatter(xr,yr)

?arc函數(shù)：輸入起點終點坐標以及起點終點角度以及點的數(shù)量

function[xr,yr,thetar,kr]=arc(init_coord,end_coord,init_angle,end_angle,count)
    L=sqrt((init_coord(1)-end_coord(1))^2+(init_coord(2)-end_coord(2))^2);
    R=L/sqrt(2*(1-cos(end_angle-init_angle)));
    delta_angle=(end_angle-init_angle)/(count-1) ;
  
       for i=1:count
           if delta_angle>0
               xr(i)=init_coord(1)-R*sin(init_angle)+R*sin(init_angle+delta_angle*(i-1));
               yr(i)=init_coord(2)+R*cos(init_angle)-R*cos(init_angle+delta_angle*(i-1));
               thetar(i)=init_angle+delta_angle*i;
               kr(i)=1/R;
           else
               xr(i)=init_coord(1)+R*sin(init_angle)-R*sin(init_angle+delta_angle*(i-1));

               yr(i)=init_coord(2)-R*cos(init_angle)+R*cos(init_angle+delta_angle*(i-1));
               thetar(i)=init_angle+delta_angle*i;
               kr(i)=-1/R;
           end               
       end  
end

用法舉例：

[xr,yr,~,kr]=arc([0,0],[50,50],0,pi/2,100);
scatter(xr,yr)

?根據(jù)這兩個函數(shù)，可以設(shè)計出一條路徑出來，并包含每個離散點的xr,yr,thetar以及kr

count=50;
[x1,y1,theta1,kr1]=straight([0,0],[20,0],0,count);
[x2,y2,theta2,kr2]=arc([20,0],[30,10],0,pi/2,count);
[x3,y3,theta3,kr3]=arc([30,10],[40,20],pi/2,0,count);
[x4,y4,theta4,kr4]=arc([40,20],[40,40],0,pi,count);
[x5,y5,theta5,kr5]=arc([40,40],[35,35],pi,3*pi/2,count);
[x6,y6,theta6,kr6]=arc([35,35],[25,35],3*pi/2,pi/2,count);
[x7,y7,theta7,kr7]=arc([25,35],[15,35],pi/2,3*pi/2,count);
[x8,y8,theta8,kr8]=arc([15,35],[5,35],3*pi/2,pi/2,count);
[x9,y9,theta9,kr9]=arc([5,35],[-15,35],pi/2,3*pi/2,count);
[x10,y10,theta10,kr10]=straight([-15,35],[-15,15],3*pi/2,count);
[x11,y11,theta11,kr11]=arc([-15,15],[0,0],3*pi/2,2*pi,count);
xr=[x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11];
yr=[y1,y2,y3,y4,y5,y6,y7,y8,y9,y10,y11];
thetar=[theta1,theta2,theta3,theta4,theta5,theta6,theta7,theta8,theta9,theta10,theta11];
kappar=[kr1,kr2,kr3,kr4,kr5,kr6,kr7,kr8,kr9,kr10,kr11];

scatter(xr,yr)

這里使用的路徑并不是好路徑，會發(fā)生曲率突變，只是用作演示用直線和圓弧簡單拼接

回到CarSim設(shè)置路面

進入Procedure

?選擇Two Lanes

?進入道路設(shè)置

設(shè)置和剛才規(guī)劃一樣的道路，先解鎖，然后Rows改成11，然后照這樣填表，

此外把Treat as loop也點上

接下來點擊Video查看建造的道路結(jié)果，注意一定要改成Flat

這個道路轉(zhuǎn)彎半徑非常小，最小只有5m，路徑非常嚴苛，可以查看我們的控制效果

接下來回到MATLAB（注意這里還是跟只前一樣，最好send to simulink），先運行一下，讓變量在工作區(qū)內(nèi)

接下來打開simulink，pid_lqr_demo里面已經(jīng)寫好了，這里重寫一遍

（2）預(yù)測模塊代碼

首先5輸入6輸出，然后單位換算，

?新建matlab 函數(shù)，預(yù)測模塊比較好寫，先寫預(yù)測模塊，把每個輸出標出來

預(yù)測模塊的算法

輸入為當(dāng)前狀態(tài)和速度v，預(yù)測時間ts

輸出為預(yù)測的狀態(tài)，算法為

代碼如下：

function [pre_x,pre_y,pre_phi,pre_vx,pre_vy,pre_phi_dot] = fcn(x,y,phi,vx,vy,phi_dot,ts)
pre_x = x + vx*ts*cos(phi) - vy*ts*sin(phi);
pre_y = y + vy*ts*cos(phi) - vx*ts*sin(phi);
pre_phi = phi+phi_dot*ts;
pre_vx = vx;
pre_vy = vy;
pre_phi_dot = phi_dot;
end

simulink里面還要輸入單位換算，ts設(shè)置為0.1

（3）誤差計算模塊代碼

根據(jù)流程圖，下一步是誤差計算模塊

輸入的為預(yù)測點信息，規(guī)劃的信息。輸出為投影點的曲率和誤差。

算法流程如下：

?代碼

function [kr,err] = fcn(x,y,phi,vx,vy,phi_dot,xr,yr,thetar,kappar)
%找到規(guī)劃軌跡的長度，即多少個規(guī)劃點，最短距離d_min，序號記為min
n = length(xr);
d_min = (x - xr(1))^2 + (y - yr(1))^2;
min = 1;
for i = 1:n
    d = (x- xr(i))^2 + (y - yr(i))^2;
    if d < d_min
        d_min = d;
        min = i;
    end
end
%找到最短點序號，按照算法一步一步來
dmin = min;
tor = [cos(thetar(dmin)); sin(thetar(dmin))];
nor = [-sin(thetar(dmin)); cos(thetar(dmin))];
d_err = [x - xr(dmin); y - yr(dmin)];
ed = nor' * d_err;
es = tor' * d_err;
% 算thetar兩種方法
projection_point_thetar = thetar(dmin); %apollo
% projection_point_thetar = thetar(dmin) + kappar(dmin) *es;
ed_dot = vy * cos(phi - projection_point_thetar) + vx * sin(phi - projection_point_thetar);
%%%%%%
%第九步這里加個sin，假設(shè)較小用正弦代替，消除±2pi的印象
ephi = sin(phi - projection_point_thetar) ;
s_dot = vx*cos(phi - projection_point_thetar) - vy * sin(phi - projection_point_thetar);
s_dot = s_dot/(1 - kappar(dmin) * ed);
ephi_dot = phi_dot - kappar(dmin) * s_dot;
kr = kappar(dmin);
err = [ed;ed_dot;ephi;ephi_dot];
end

其中xr，yr，thetar，kappar用from workspace模塊導(dǎo)入（運行simulink的時候必須提前工作空間內(nèi)有，如果避免麻煩都弄成行向量）

（4）LQR模塊代碼

下面寫LQR模塊，matlab有自帶的lqr包，可以直接使用

首先定義好參數(shù)，然后使用matlab自帶的lqr包算出來K，然后拆成k1,k2,k3,k4

cf  = -110000;
cr = cf;         %差別不大，近似認為是一樣的
m = 1412;   %質(zhì)量，包含簧上質(zhì)量和懸架質(zhì)量
Iz = 1536.7;  
a = 1.015;
b = 2.910 - 1.015;
k = zeros(5000,4); %LQR 的K矩陣
for i = 1:5000
      vx = 0.01*i; %每隔0.01秒刷新一下K，0-50m/s
      A = [0,1,0,0;
          0,(cf+cr)/(m*vx),-(cf+cr)/m,(a*cf-b*cr)/(m*vx);
          0,0,0,1;
          0,(a*cf-b*cr)/(Iz*vx),-(a*cf-b*cr)/Iz,(a*a*cf+b*b*cr)/(Iz*vx)];
      B = [0;
          -cf/m;
          0;
          -a*cf/Iz];
      Q= eye(4);
      R = 100;
      k(i,:) = lqr(A,B,Q,R); %k就是離線lqr的表
end
%由于Simulink沒法傳矩陣，只能傳行向量  
k1 = k(:,1)';
k2 = k(:,2)';
k3 = k(:,3)';
k4 = k(:,4)';

simulink新建一個lqr函數(shù)，根據(jù)vx來選擇對應(yīng)的k

function k  = fcn(k1,k2,k3,k4,vx)
if abs(vx) < 0.01
    k = [0,0,0,0];
else
    index = round(vx/0.01);
    k = [k1(index),k2(index),k3(index),k4(index)];
end

end

運行一下獲取四個k，然后輸入進lqr

?（5）前饋控制模塊代碼

接下來是算前饋的模塊，需要整車參數(shù)，vx，kr（投影點的曲率），直接帶公式

?代碼

function forward_angle = fcn(a,b,m,vx,cf,k,kr)
forward_angle = kr*(a+b-b*k(3)-(m*vx*vx/(a+b))*(b/cf)+(a/cr)*k(3)-(a/cr));
end

（6）整合模塊代碼

最后一個整合模塊

function angle = fcn(k,err,forward_angle)
angle = -k * err + forward_angle;
end

?（7）仿真運行

此時所有代碼寫完了，打包封裝一下，選中SF輸出的模塊 Crtl+G打包

將輸出的angle反饋給CarSim，注意弧度轉(zhuǎn)換為角度，CarSim是角度

再加一個延遲模塊，作用是：如果輸入是這一時刻的值，輸出是上一時刻的值。等于延遲一個單元，為了避免代數(shù)環(huán)。

節(jié)氣門開度給個0.15，后輪轉(zhuǎn)角給個0

此外給angle加一個限制防止角度太大超過物理限制，再加一個示波器查看angle變化

此外，將初速度調(diào)成不設(shè)置初速度（默認120），因為速度過大，道路嚴苛，很難控制容易報錯

點擊run，使用XY-graph查看

?X范圍有點小，調(diào)大

這個時候點擊run

仿真結(jié)果如圖所示（注意如果時長短是因為上面Procedure里面改成10s仿真了，只需要改成50S然后再Send to Simulink即可）

回到CarSim點擊Video查看實際運行視頻

前輪轉(zhuǎn)角不停抖來都去

?改進方法

計算thetar的時候，加上es，這樣就可以防止大抖動，減少抖動

但是這種方法沒法解決突變問題，因為規(guī)劃的曲率突變，控制很難解決，得在規(guī)劃的時候進行插值讓曲率連續(xù)，要不然就把ＬＱＲ的Ｒ調(diào)大一點

另外，不能讓規(guī)劃的點過少，這樣會讓車圍繞一個點不停打轉(zhuǎn)，因為匹配點不會改變

第九講? 縱向控制開篇

縱向控制比較簡單，不需要太多的數(shù)學(xué)知識

一、縱向控制邏輯

首先要明白：當(dāng)你踩下油門/剎車時，你到底在控制什么？

橫向控制邏輯：方向盤-->前輪轉(zhuǎn)角-->車的航向角

縱向控制：油門/剎車--> ？--> 車加速/減速

這里的？到底是什么，到底什么被改變了，導(dǎo)致車速改變

直觀上想到發(fā)動機轉(zhuǎn)速或者發(fā)動機扭矩

正確答案：油門-->發(fā)動機功率被改變-->?-->車加速

剎車-->制動壓力增大-->制動盤摩擦增大-->阻力增大-->車減速

功率與車加速不直觀

什么與車加速直接相關(guān)？車速加大，只需要加速度大于0即可，車速與發(fā)動機轉(zhuǎn)速相關(guān)，而加速度與發(fā)動機扭矩相關(guān)

縱向控制邏輯：油門 --> 功率 --> 轉(zhuǎn)速 / 扭矩 --> 車速 / 車加速度 --> 車加速

當(dāng)踩油門時，到底在控制什么？

P與M，ω之間的關(guān)系：? ?

真實的發(fā)動機扭矩轉(zhuǎn)速功率之間的關(guān)系需要用實驗曲線來獲得

二、發(fā)動機與變速器

發(fā)動機扭矩-轉(zhuǎn)速曲線

并不呈反比例曲線，因為P一部分用于發(fā)熱，低速情況熱占主導(dǎo)，高速情況做功占主導(dǎo)。

所以發(fā)動機最好只在高效/高扭矩區(qū)運行，發(fā)揮其最大優(yōu)勢，但是其高效區(qū)很短

變速器應(yīng)運而生，?把發(fā)動機轉(zhuǎn)速降低

?變速器通過齒輪切換從而將發(fā)動機的高效轉(zhuǎn)速得以拓寬

?齒輪切換術(shù)語叫換擋：一檔二檔三檔四檔五檔

一檔最慢，扭矩最大，用于啟動爬坡

五檔最快，扭矩最小，用于高速

發(fā)動機與電機的轉(zhuǎn)速扭矩圖

電機在前面就有很大扭矩，最高轉(zhuǎn)速16000，高效區(qū)間長，效率最高90%

發(fā)動機在前面運行，最高轉(zhuǎn)速6500，高效區(qū)間很短，效率最高40%

因此發(fā)動機一定需要變速器，電機不一定需要變速器

發(fā)動機低速差，高速較強。電機在低速加速極強，高速差。

我們的控制基于電動汽車，電動車控制簡單且為大勢所趨，不需要考慮換擋策略

電機工作區(qū)間分為恒扭矩區(qū)和恒功率區(qū)，在到達最大轉(zhuǎn)速前扭矩不變，到達之后功率不變

三、CarSim基礎(chǔ)設(shè)置

由于CarSim沒有電機，所以需要造個電機，下面為假設(shè)

電機參數(shù)：最大扭矩380Nm，最大功率180kW

?估算出其最大轉(zhuǎn)速

?M與ω之間的關(guān)系為

?雖然是假設(shè)的電機，但是方法是通用的，不管電機參數(shù)如何，都通用。

我們的方法是通過實驗標定來的，哪怕曲線亂七八糟的也有效果

下面進入CarSim設(shè)置電機

先新建一個模型

?設(shè)置輸入為發(fā)動機扭矩

?輸出為Vx，Ax以及曲軸的轉(zhuǎn)速

?接下來send to simulink

設(shè)置電機模型

function torque = fcn(power,rpm)
Tmax = 380*power;
if(rpm <= 4523)
    torque = Tmax;
else
    torque = Tmax * 4523/rpm;
end
end

接下來進入CarSim的C-class

?進入變速器設(shè)置

?新建一個變速器模型

?將傳動比，慣量和效率都改成如下，和電機匹配

?進入發(fā)動機配置，可以通過更改發(fā)動機曲線來模擬電機，也可以用Simulink

設(shè)置好后，一定要Send to simulink，要不然都白設(shè)置了

設(shè)置油門給1，然后速度和加速度都轉(zhuǎn)換好

?得到速度和加速度曲線，

?最大速度超過45m/s，符合家用車最高車速

油門調(diào)成0.1就不會那么快了

?油門調(diào)成0，仍然有速度，因為發(fā)動機有怠速

?怠速設(shè)置在發(fā)動機曲線內(nèi)，750rpm

?以前的電動機無怠速，現(xiàn)在的電動機都給個怠速，因為如果沒有怠速，電機加速度過大，啟動速度過大會不平順。我們這里改成0改成750都行，選擇不改。

下面是路面修改，不需要之前的彎道，只需要一個2000m的道路即可（注意這里把loop取消勾，然后把前面path終點也改成2000，否則后面simulink報錯）

?最后再把Simulink的文件名改一下

OK，至此準備工作完畢，具體的仿真后續(xù)第十講會做

第十講 油門剎車標定表的制作

一、理論部分

找到油門和v，a對應(yīng)的關(guān)系

?做實驗，踩不同的油門，得到不同的v，a曲線

對于一個thr，不同的時間t得到一系列va點，v，a可以合并，得到v，a曲線

?不同的thr會得到不同的va曲線，

?使用不同的thr做實驗，可以得到一個三維曲面

通過做實驗，得到大量的（v，a，thr）的三維點，從而擬合出thr = f(v,a)

f(v,a)叫做模型（深度學(xué)習(xí)術(shù)語）

標定表的制作

二、油門標定表制作

來到CarSim把剎車制動壓力加上去

來到Simulink，首先給mux加一個輸入（剎車，這里設(shè)為0），然后函數(shù)輸入的油門用一個變量來表示，使用to workspace創(chuàng)建兩個變量vx和ax

?準備就緒后，就開始寫標定算法，回到matlab創(chuàng)建一個文件

?這個時候vx和ax就傳進matlab里面了，simulink里面也能看到

?

?這時候發(fā)現(xiàn)采樣太密了，改成0.1秒采樣，注意這里要點apply，否則matlab報錯

?接下來開始畫表

在matlab中使用以下代碼，

thr = 0;  %初始化油門
for i = 1:11
    sim('calibration');
    v_temp(:,i) = vx.data;
    a_temp(:,i) = ax.data;
    thr_temp(:,i) = ones(length(vx.data),1)*thr;
    thr = thr + 0.1;
end

注意，這代碼運行時間比較長，而且如果設(shè)置的時間過長的話，車速很快時，會率先到達2000m停下來，此時矩陣維度不一致會報錯，所以將仿真時間截至調(diào)到30s以內(nèi)

?跑完之后合并起來

% 合并，一定要轉(zhuǎn)成行向量再合并，否則會導(dǎo)致合并失敗
v = v_temp(:,1)';
a = a_temp(:,1)';
tr = thr_temp(:,1)';
for i = 2:11
    v = [v, v_temp(:,i)'];
    a = [a, a_temp(:,i)'];
    tr = [tr, thr_temp(:,i)'];
end

然后再擬合

%擬合
F = scatteredInterpolant(v',a',tr');% 轉(zhuǎn)成列向量
vu = 0:0.1:50;
au = 0:0.1:5;
table = zeros(length(vu),length(au));
for i = 1:length(vu)
    for j = 1:length(au)
        table(i,j) = F(vu(i),au(j));
    end
end

運行之后得到table

?接下來回到Simulink，使用2D lookup table擬合

然后改成au，vu和table，

?接下來點擊Edit table and breakpoints查看擬合圖像

?在Simulink內(nèi)連接好速度和加速度，給1的加速度，點擊run，得到其實際加速度也差不多為1

?改成3后，發(fā)現(xiàn)剛開始還可以正常跑，后面就不對了，因為油門超過1了，實際上油門最大時1

?這個table是用不超過1的油門來擬合的，所以需要加限制，在加一個延遲模塊防止代數(shù)環(huán)

下面是速度控制，控制在10，注意這里加速度輸入為車速與控制速度相減

?如果直接用車的加速度輸入的話，結(jié)果不準確。

原因，速度與加速度不匹配，要讓加速度慢慢降低至0。上面使用目標速度與實際速度相減的信號作為輸入，這樣會讓加速度慢慢降到0，匹配上了，控制效果就會好很多。

無論是速度控制還是加速度控制，都要匹配，不能亂寫。

三、剎車標定表制作

剎車標定和油門差不多

首先進入Simulink，油門設(shè)置為0，剎車設(shè)置一個變量

?初速度設(shè)置為最快的速度，這里設(shè)置為180

?send to simulink，開始寫代碼（油門和剎車的差不多），這里設(shè)置為0-8s的數(shù)據(jù)

brake = 0;  %初始化油門
for i = 1:80
    % 該程序非常耗時，如果需要更多更密集的數(shù)據(jù)，請先測試
    sim('calibration');
    v_temp(:,i) = vx.data;
    a_temp(:,i) = ax.data;
    brake_temp(:,i) = ones(length(vx.data),1)*brake;
    brake = brake - 0.1;
end

合并，80個數(shù)據(jù)進行合并

% 合并，一定要轉(zhuǎn)成行向量再合并，否則會導(dǎo)致合并失敗
vbr = v_temp(:,1)';
abr = a_temp(:,1)';
br = brake_temp(:,1)';
for i = 2:80
    vbr = [vbr, v_temp(:,i)'];
    abr = [abr, a_temp(:,i)'];
    br = [br, brake_temp(:,i)'];
end

擬合，這里注意加速度為負的（剎車），v和a都為單調(diào)遞增的，否則不識別

%擬合
F = scatteredInterpolant(vbr',abr',br');% 轉(zhuǎn)成列向量
vubr = 0:0.05:50;
aubr = -8:0.05:0;  %注意這里加速度和之前不一樣，為負的
tablebr = zeros(length(vubr),length(aubr));
for i = 1:length(vubr)
    for j = 1:length(aubr)
        tablebr(i,j) = F(vubr(i),aubr(j));
    end
end

點擊運行，程序很耗時，跑完得到tablebr ，豎著為速度0-50，橫著為加速度-8到0

最后一列的數(shù)值突然變得很大，對應(yīng)的是v和a=0的情況，停車狀態(tài)，這個時候任意制動都會導(dǎo)致a=0或者v=0，這個點是所有曲線的交點，有奇異性，matlab取了個平均值。這里改一下，改成0.3，假設(shè)僅有手剎。（注：這里圖有點問題，因為代碼寫的有點錯誤，實際161列第二行開始應(yīng)該為0.3左右，把第一行也改成0.3即可）

?再設(shè)置一個lookup2D，把我們的數(shù)據(jù)輸入進去

?設(shè)置剎車加速度為-3，看看效果

?效果不錯，以-3加速度制動，然后停車后加速度為0。

以目標速度為0，然后當(dāng)前速度與目標速度相減輸入給加速度，這樣也可以

注意加一個制動壓力限制，最大9最小0MPa

?最后速度與加速度都收斂至0

?可以返回CarSim查看車輛運行狀態(tài)

第十一講 縱向雙PID控制

一、前置準備

首先，這里開始使用CarSim 2019版本，縱向控制沒法用2016版本。

變速器調(diào)整：換成一個擋位，然后換擋時間調(diào)短

?初速度為0，仿真時間調(diào)長一點，這里設(shè)置為40S

這里不要忘記吧剎車和轉(zhuǎn)向都關(guān)掉，這些都由我們自己在simulink自己設(shè)置，

然后把道路改成Double Lane，隨便設(shè)置一個長度，這里設(shè)置成5000m

接下來Send to Simulink

首先要做的是把油門和剎車做到一張表上，會自動插值，這樣會更加平順

設(shè)置一個變量x為油門剎車，x大于0為油門，小于0為剎車，使用一個函數(shù)用來判斷

function [thr,brake] = fcn(x)
% 正數(shù)代表油門，負數(shù)代表剎車
%不允許同時踩油門和剎車
if x  >= 0
    thr =x;
    brake = 0;
else
    thr = 0;
    brake = -x;
end
end

接下來是標定代碼（油門和剎車分開寫，因為初速度不一樣）

thr_calibration.m 油門采集數(shù)據(jù)代碼

x = 0;  %初始化油門
for i = 1:21
    sim('calibration');
    v_temp(:,i) = vx.data;
    a_temp(:,i) = ax.data;
    thr_temp(:,i) = ones(length(vx.data),1)*x;
    x = x + 0.1;
end

% 合并，一定要轉(zhuǎn)成行向量再合并，否則會導(dǎo)致合并失敗
v = v_temp(:,1)';
a = a_temp(:,1)';
tr = thr_temp(:,1)';
for i = 2:length(v_temp(1,:))
    v = [v, v_temp(:,i)'];
    a = [a, a_temp(:,i)'];
    tr = [tr, thr_temp(:,i)'];
end

brake_calibration.m 剎車采集數(shù)據(jù)代碼

% 啟動前檢查CarSim的初速度是否為180
x = 0;  %初始化剎車
%%% 剎車的初速度一定要比較高，180km/h，144km/h
for i = 1:81
    % 該程序非常耗時，如果需要更多更密集的數(shù)據(jù)，請先測試
    sim('calibration');
    v_temp1(:,i) = vx.data;
    a_temp1(:,i) = ax.data;
    brake_temp1(:,i) = ones(length(vx.data),1)*x;
    %%% 這里是為了消除奇異性，因為無論brake=1還是2，最后都會導(dǎo)致車的v=0，a=0，這將導(dǎo)致多值性
    for j = 1:length(v_temp1(:,i))
        if v_temp1(j,i)<0.01
            brake_temp1(j,i) = 0;
        end
    end
    x = x - 0.1;
end

% 合并，一定要轉(zhuǎn)成行向量再合并，否則會導(dǎo)致合并失敗
vbr = v_temp1(:,1)';
abr = a_temp1(:,1)';
br = brake_temp1(:,1)';
for i = 2:80
    vbr = [vbr, v_temp1(:,i)'];
    abr = [abr, a_temp1(:,i)'];
    br = [br, brake_temp1(:,i)'];
end

二、油門和剎車標定表制作

首先回到CarSim把初速度改成0，然后運行油門代碼獲得油門標定表

然后再回到CarSim把初速度改成180，然后運行剎車代碼，獲得剎車標定表

剎車標定表第一行全部都是-7.032，由于初始化問題，我們把第二行數(shù)據(jù)賦值給第一行

?加上代碼

a_temp1(1,:) = a_temp1(2,:);

把上面Sim部分注釋掉，然后重跑一下

代碼generate_callibration.m（把油門和剎車都合并起來）

v2 = [v,vbr];
a2 = [a,abr];
br2 = [tr,br];

%擬合
F = scatteredInterpolant(v2',a2',br2');% 轉(zhuǎn)成列向量
vubr = 0:0.05:50;
aubr = -8:0.05:5;  %注意這里加速度為-8到正5
tablebr = zeros(length(vubr),length(aubr));
for i = 1:length(vubr)
    for j = 1:length(aubr)
        tablebr(i,j) = F(vubr(i),aubr(j));
    end
end

在Simulink把這個表使用lookup2D設(shè)置好

?把表，轉(zhuǎn)換模塊，電機模型整合一下。輸入輸出變量名設(shè)置好

把CarSim初速度改成0，然后Send一下

三、縱向速度PID控制仿真

假設(shè)我們要控制加速度為10，使用簡單的pid控制：期望速度-當(dāng)前速度作為加速度進行輸入

?仿真查看v運行結(jié)果，發(fā)現(xiàn)沒反應(yīng)，因為我們加速度沒有給限制，表格內(nèi)沒有10這么大的加速度

?加一個限制

這個時候在運行，查看v確實有這個控制效果

?仔細看發(fā)現(xiàn)并沒有到10，因為接近10的時候加速度平緩導(dǎo)致不加速了

?改進辦法：加一個比例項，PID的P就是比例，I是積分，D是微分

?可以看到，收斂速度更快，結(jié)果也好一些。P可以加快收斂

比例項也不是越大越好，這里調(diào)到10，發(fā)現(xiàn)出現(xiàn)超調(diào)。

?把比例改成3，然后看下180減速到10會怎么樣，可以看到發(fā)生震蕩

改成pid，這里給0.1積分，發(fā)現(xiàn)會出現(xiàn)超調(diào)，但是最后還是會收斂到10

I主要目的：消除穩(wěn)態(tài)誤差，但是可能會引起超調(diào)

?I改成1后，發(fā)現(xiàn)超調(diào)嚴重，但是仍然能回到10，因為I存在就會對誤差消除

?現(xiàn)在吧積分去掉，然后加一點微分D，微分的作用是抑制超調(diào)

這里放大并與上面不加微分的對比，發(fā)現(xiàn)震蕩和超調(diào)被抑制

所以如果誤差不是特別大的情況，?一般用PD即可，因為I會引起超調(diào)。

下面設(shè)計一段真實的軌跡，在Simulink中新建一個函數(shù)

function y = fcn(t)
if t < 10
    s = 0.1* t^3/3;
    v = 0.1*t^2;
    a = 0.2*t;
else
    a = 2 - 0.1*(t-10);
    v = 2*t - 0.05*(t-10)^2-10;
    s = t^2 - 0.05*(t-10)^3/3-10*t+100/3;
end
end

檢測一下是否連續(xù)

?速度一直在增加，所以仿真時間要加長，這里設(shè)置到60

此外要把車的坐標輸出出來

接下來回到Simulink，速度和位置都需要滿足約束，所以把速度和位置的誤差都輸入進去，位置PID限制±10，此外加速度信號也連上，這樣雙PID的縱向控制就搭建完了。

?注意這里的PID參數(shù)為比例6，積分0，微分0.1

?查看結(jié)果，位置還可以

?速度和加速度就不太行

?可能是初速度的緣故，現(xiàn)在把初速度調(diào)成0，看下結(jié)果

可以看到位置還可以

?加速度不咋樣

?速度有點“畫龍”，一會兒大一會兒小

下面把速度PID的比例項調(diào)小點，調(diào)成2?

速度“畫龍”的現(xiàn)象得到改善?

?加速度也比較好，后面的波動為停車后的

但是加速度前面還不太好，需要調(diào)參，直接給出參數(shù)

速度PID這樣調(diào)

?位置PID這樣調(diào)

速度和加速度結(jié)果如圖

?最后打包一下

第十二講? 橫縱向綜合控制完結(jié)

1-2 開篇，運動學(xué)方程

3-8 橫向控制 LQR

9-11 縱向控制 雙PID

12 橫縱向控制，規(guī)劃接口

運動學(xué)方程：，適用于低速情況，轉(zhuǎn)角大小均可

LQR適用于小轉(zhuǎn)角，低速高速均可以

所以一般在自動泊車等低速大轉(zhuǎn)角的場景用運動學(xué)方程控制

一、規(guī)劃接口

控制模塊功能：接受一條規(guī)劃的軌跡，讓車按照規(guī)劃的軌跡運動。首先得有規(guī)劃，然后在進行橫縱向控制。

注意這里是軌跡規(guī)劃，不是路徑規(guī)劃，在自動駕駛里面，路徑只會告訴該怎么走，與時間無關(guān)，而軌跡規(guī)劃包含時間，速度，加速度，曲率等信息。

軌跡規(guī)劃：x(t)，y(t)。（先做直角坐標系下的，由簡單開始）

假設(shè)有下面的坐標系，起點在原點，終點在（100，10）處

?那么有停車場景

?或者駛?cè)雸鼍埃ńK點有速度20）

?或者超車場景，起點速度為10，終點速度為20

今天就研究給定起點終點速度，加速度信息的規(guī)劃。（最簡單，最常用，覆蓋大部分直線場景，但是不能做轉(zhuǎn)彎和掉頭，轉(zhuǎn)彎掉頭必須在自然坐標系下）。

這個地方的規(guī)劃主要是為了控制服務(wù)。

規(guī)劃任務(wù)可以描述為一個數(shù)學(xué)問題：設(shè)計一條合適的x(t），y(t)，滿足始末的邊界條件：

其中，T為耗費的時間

機器人領(lǐng)域有這種算法，但是無人駕駛不能直接照搬，因為車不能單獨做橫向運動，橫向運動通常由縱向運動誘發(fā)

規(guī)劃的軌跡有切線，曲率，加速度，速度的限制

?如果在起點建立一個直角坐標系，那么對也有要求

?汽車規(guī)劃的邊界條件為

機器人的x與y都與時間相關(guān)，而無人車不能做縱向運動，所以y和x有關(guān)

僅僅用y'做規(guī)劃還不夠，y與x相關(guān)，要把它弄成與時間相關(guān)

所以還需要y(t)與y(x)的轉(zhuǎn)化

求y'和y''的時候使用復(fù)合求導(dǎo)

?邊界條件為

先算出y(x)再轉(zhuǎn)化為y(t)

對于x(t)使用五次多項式

?五個系數(shù)正好對應(yīng)5個邊界條件

對于y(t)也使用五次多項式

?因為y也有五個邊界條件

通過邊界條件求出多項式系數(shù)a0-5，b0-5

通過解出y(t)，y'，y''

橫向控制與規(guī)劃的接口

第八講橫向控制中有，，，

其中xr,yr代表匹配點，代表軌跡切線與x軸的夾角，代表軌跡曲率

xr,yr就是規(guī)劃器中的x(t)，y(t)。

?，可用下式算出

縱向控制與規(guī)劃的接口

在橫向控制里有，es為下圖紅色向量在藍色向量上的投影

縱向誤差：目標點與當(dāng)前點之間的誤差。在此圖中es作為誤差應(yīng)為正，所以橫向控制的es輸入到縱向應(yīng)該加一個負號

速度誤差：，在橫向控制中有

?期望加速度

邏輯梳理

首先根據(jù)規(guī)劃獲得x(t)，y(t)

根據(jù)x(t)，y(t)我們可以得到

?根據(jù)這些我們可以得到

?縱向控制前兩個是從橫向控制得到，實際輸入只有后三個

二、代碼實踐部分

首先是橫向控制和縱向控制代碼和模型文件

ch8和ch11的代碼和模型復(fù)制到CarSim目錄下面

然后打開Matlab

注意lqr這里的cf和cr要改一下，因為需要更精確的模型，不能像第八講那樣混一下，

?必須要計算出每個輪子精確的垂向力，然后通過垂向力查表查到側(cè)偏剛度大概估算出來

?不要忘了加這句話

?重新設(shè)置一個新的模型 planning_control

?INPUT設(shè)置

?注意這里一定要調(diào)成replace

?OUTPUT設(shè)置

?新建Simulink模型然后Carsim綁定這個模型

?仿真時間改成40S，初速度為0，剎車和轉(zhuǎn)向不開啟

把縱向控制模型復(fù)制進去

?下面進行標定，這里把輸入輸出改成上一講的輸入輸出，

解除注釋，然后再重復(fù)上一講的內(nèi)容進行table制作

如果懶得再弄一遍，這里直接上鏈接

油門剎車標定表.zip - 藍奏云

把mat文件在matlab中加載一下

接下來把縱向雙PID搭建好，然后把輸出改成7個，刪掉標定的東西，然后標上實際輸出

?接下來把左下角的規(guī)劃模塊改一下，改一下輸出

?接下來把位置PID的輸入改成es，然后把輸出加上單位換算（注意yaw那里是pi/180，之后才發(fā)現(xiàn)錯誤），在進行一些調(diào)整

?接下來把如圖所示的模塊都合并一下

要把速度當(dāng)成電機輸入，所以下面寫一個速度函數(shù)

function v = fcn(vx,vy)
    v = sqrt(vx^2 + vy^2);
end

然后我們把規(guī)劃重新寫一遍

function [vp,ap,xr,yr,thetar,kr] = fcn(t)
% 在20s，向前移動100米，向左移動10m
dx = 100;
dy = 10;
T = 20;
%起點終點速度和加速度
xstart = [0,0,0];
xend = [dx,0,0];
ystart = [0,0,0];
yend = [dy,0,0];
% 設(shè)置多項式系數(shù)向量
a = zeros(1,6);
b = zeros(1,6);
%解x的系數(shù)
a(1) = xstart(1);
a(2) = xstart(2);
a(3) = xstart(3)/2;
A1 = [T^3,       T^4,      T^5;
          3*T^2,   4*T^3,   5*T^4;
          6*T,       12*T^2,  20*T^3];
B1 = [xend(1)-a(1)-a(2)*T-a(3)*T^2;
          xend(2)-a(2)-2*a(3)*T;
          xend(3)-2*a(3)];
xs = inv(A1)*B1;
a(4) = xs(1);
a(5) = xs(2);
a(6) = xs(3);
%解y的系數(shù)
b(1) = ystart(1);
b(2) = ystart(2);
b(3) = ystart(3)/2;
A2 = [dx^3,       dx^4,      dx^5;
          3*dx^2,   4*dx^3,   5*dx^4;
          6*dx,       12*dx^2,  20*dx^3];
B2 = [yend(1)-b(1)-b(2)*dx-b(3)*dx^2;
          yend(2)-b(2)-2*b(3)*dx;
          yend(3)-2*b(2)];
ys = inv(A2)*B2;
b(4) = ys(1);
b(5) = ys(2);
b(6) = ys(3);
% 求x點，x兩點，y‘，y’‘以及曲率，vp，ap
xr = a(1) + a(2)*t + a(3)*t^2 + a(4)*t^3 + a(5) *t^4+a(6)*t^5;
yr = b(1) + b(2)*xr + b(3)*xr^2 + b(4)*xr^3 + ab(5) *xr^4+b(6)*xr^5;
xr_dot = a(2) + 2*a(3)*t + 3*a(4)*t^2 + 4*a(5) *t^3+5*a(6)*t^4;
yr_dx =  b(2) + 2*b(3)*xr + 3*b(4)*xr^2 + 4*ab(5) *xr^3+5*b(6)*xr^4;
yr_dot = yr_dx *xr_dot;
thetar = atan(yr_dx);
xr_dot2 = 2*a(3)+6*a(4)+ 12*a(5) *t^2+20*a(6)*t^3;
yr_dx2 = 2*b(3)+6*b(4)+ 12*b(5) *xr^2+20*b(6)*xr^3;
yr_dot2 = yr_dx2*xr_dot^2 + yr_dx*xr_dot2;
kr = yr_dx2/((1 + yr_dx^2)^1.5);
vp = sqrt(xr_dot^2 + yr_dot^2);
ap = sqrt(xr_dot2^2 + yr_dot2^2);
end
    

重新連一下規(guī)劃的線

?接下來把橫向控制加進來

打開橫向控制的模型

?下面對主要模塊進行修改

?去掉單位換算，然后把xr，yr，thetar，kappar都設(shè)置為外部輸入

?接下來更改誤差計算模塊，增加輸出es和s_dot，然后去掉尋找dmin的過程，因為我們這里的規(guī)劃不是之前那樣給好軌跡

function [kr,err,es,s_dot] = fcn(x,y,phi,vx,vy,phi_dot,xr,yr,thetar,kappar)
    tor=[cos(thetar);sin(thetar)];
    nor=[-sin(thetar);cos(thetar)];
    d_err=[x-xr;y-yr];
    ed=nor'*d_err;
    es=tor'*d_err;
    %projection_point_thetar=thetar(dmin);%apollo
    projection_point_thetar=thetar+kappar*es;
    ed_dot=vy*cos(phi-projection_point_thetar)+vx*sin(phi-projection_point_thetar);
    %%%%%%%%%
    ephi=sin(phi-projection_point_thetar);
    %%%%%%%%%
    ss_dot=vx*cos(phi-projection_point_thetar)-vy*sin(phi-projection_point_thetar);
    s_dot=ss_dot/(1-kappar*ed);
    ephi_dot=phi_dot-kappar*s_dot;
    kr=kappar;
    err=[ed;ed_dot;ephi;ephi_dot];
end

接下來退出來，先把該連的連起來，然后把輸入改成6個，增加前后輪轉(zhuǎn)角，后輪轉(zhuǎn)角設(shè)置為0，然后給angle加上單位換算和±1的限制，給es乘以-1輸入到縱向控制

接下來到縱向控制內(nèi)，給es增加限制±10，然后PID參數(shù)改成如下

?接下來增加2個示波器，比較一下實際x,y和規(guī)劃的xr,yr有多大誤差?

?進入matlab運行l(wèi)qr_offline獲得k

接著運行Simulink?

報錯，原因：仿真時間過長

Index exceeds array dimensions. Index value 5000 exceeds valid range [1-4999] of array k1. Error in 'planning_control/Subsystem1/lqr_offline' (line 6) k=[k1(index),k2(index),k3(index),k4(index)];

調(diào)短一點再run，然后查看scope，發(fā)現(xiàn)x，y基本上重合

放大看還是有0.幾的誤差，仿真中對誤差的要求是0.0幾m以內(nèi)才能保證實車上面有誤差和噪聲的條件下精確

原因分析：在planning模塊中，ap可正可負

所以將其中的

ap=sqrt(xr_dot2^2+yr_dot2^2);

改為ap根據(jù)xr_dot2的正負改變

if xr_dot2>=0
    ap=sqrt(xr_dot2^2+yr_dot2^2);
else
    ap=-sqrt(xr_dot2^2+yr_dot2^2);
end

調(diào)好后再run一下發(fā)現(xiàn)橫向誤差還是大，縱向誤差可以接受

發(fā)現(xiàn)是angle這里應(yīng)該先限制再單位轉(zhuǎn)換

為了能直觀看到橫向誤差，我們在LQR模塊把ed輸出出來

?差不多ed是0.04左右

?去規(guī)劃中把速度調(diào)快，10s內(nèi)完成

?發(fā)現(xiàn)高速下性能不怎么樣

?原因：加速度ap超了

下面試一下低速大轉(zhuǎn)角

?回到CarSim把仿真時間改到80

?再回到simulink加一個scope查看速度

?表現(xiàn)得很好

橫向縱向誤差均在0.05以內(nèi)

lqr需要在小轉(zhuǎn)角，加速度不能超了，一般在2-3以內(nèi)

一般橫向誤差控制的不好，原因有三

一是側(cè)偏剛度自己估的，不太準，前輪后輪垂向力不一樣，因為加速度導(dǎo)致軸荷轉(zhuǎn)移，垂向力不一樣導(dǎo)致側(cè)偏剛度變化

二是因為LQR基于二自由度模型，自行車模型，本來就有簡化性，本來就有誤差

三是汽車固有特性：轉(zhuǎn)向不足

一般來說仿真達到厘米級可以接受，實車需要達到0.1m可以接受，因為實車有誤差和噪聲

如果橫向誤差過大，需要給Q更大的懲罰值，即給ed更大的懲罰值，盡可能讓ed收斂到0，而e_phi不可能收斂到0，因為e_phi的穩(wěn)態(tài)誤差就是beta

一般來說改三個，一個是轉(zhuǎn)向不足，二一個是LQR的側(cè)偏剛度，三一個是調(diào)LQR的Q

三、轉(zhuǎn)向不足

最后講一下轉(zhuǎn)向不足導(dǎo)致的橫向誤差過大問題

轉(zhuǎn)向不足和過度轉(zhuǎn)向都是實車會發(fā)生的情況，即實際轉(zhuǎn)角小于或者大于理論轉(zhuǎn)角

?為什么會發(fā)生轉(zhuǎn)向不足\過度呢

如果前后輪側(cè)向力不匹配，質(zhì)心處會存在力矩

?如果，質(zhì)心無力矩，導(dǎo)致中性轉(zhuǎn)向

如果，質(zhì)心有正力矩，導(dǎo)致過度轉(zhuǎn)向

如果，質(zhì)心有負力矩，導(dǎo)致不足轉(zhuǎn)向

一般市面上買到的車都是轉(zhuǎn)向不足，為了安全考慮，一般賽車調(diào)校成中性轉(zhuǎn)向，因為賽車天生高速下有過度轉(zhuǎn)向的趨勢

那么如何處理呢？

方法：使用PID，給誤差做個積分，再補償?shù)角拜嗈D(zhuǎn)角上去

這里PID給I設(shè)置為3，其他均0。此外把單位轉(zhuǎn)換放到后面去，先做減法再換算。

?因為ed為正說明方向盤打多了，就給它減掉，

下面把規(guī)劃出的橫向位置調(diào)的嚴苛一點

?然后仿真30秒查看結(jié)果

效果更好一點

?誤差更小一些，橫向誤差控制在了0.02以內(nèi)

查看ed ，ed已經(jīng)在0.01以內(nèi)了

?這里因為模型本來比較準，所以提升不大，但是對于實車而言提升比較大

到這里本課程基本結(jié)束

結(jié)論：matlab速度太慢了，而且這里只有純控制無規(guī)劃，加上規(guī)劃，決策等模塊更慢，所以實車還是需要C++和linux，因為速度會更快文章來源地址http://www.zghlxwxcb.cn/news/detail-420162.html

到了這里，關(guān)于【自動駕駛】【零基礎(chǔ)】基礎(chǔ)自動駕駛控制算法筆記的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！