《車聯(lián)網(wǎng)技術(shù)與應(yīng)用》課件-無人駕駛路徑跟蹤及算法實現(xiàn)

汽車與工程機(jī)械學(xué)院無人駕駛路徑跟蹤及算法實現(xiàn)課程內(nèi)容一二路徑跟蹤PID控制算式的數(shù)字化三卡爾曼濾波與擴(kuò)展卡爾曼濾波EKF一、路徑跟蹤一、綜述路徑跟蹤問題一直是無人駕駛車輛研究的關(guān)鍵問題之一。反映的是一種使無人駕駛車輛穩(wěn)定且無偏差地沿著已規(guī)劃路徑行駛的能力。主要解決以下兩個問題：無人車行駛路徑與已規(guī)劃路徑的偏差問題。反饋信號存在滯后、滯環(huán)等特性，如何保證無人車輛系統(tǒng)實時性和穩(wěn)定性的要求。一、路徑跟蹤二、基于當(dāng)前偏差的路徑跟蹤技術(shù)所謂基于當(dāng)前偏差的路徑跟蹤技術(shù)，即基于車輛當(dāng)前位置與期望路徑之間側(cè)向距離偏差與方位偏差的位置偏差反饋控制系統(tǒng)。幾種技術(shù)：運動學(xué)以當(dāng)前路徑信息作為反饋，以未來路徑信息作為預(yù)瞄，設(shè)計了預(yù)瞄加反饋的控制器，預(yù)瞄距離和智能車速度根據(jù)預(yù)瞄路徑的彎曲程度自動調(diào)整。一種基于車輛位置誤差模型的積分誤差Back-stepping控制方法。這些都是以車輛前方或當(dāng)前位置與道路的位置偏差作為輸入，通過各種反饋控制方法設(shè)計車輛運動學(xué)反饋控制系統(tǒng)。由于沒有考慮車輛的動力學(xué)特性，其無法滿足實際要求。同時由于車輛系統(tǒng)時間滯后性，控制過程中會存在大的延遲環(huán)節(jié)，因此存在控制精度不高、實時性差的缺點。一、路徑跟蹤幾種技術(shù)一種結(jié)合補償控制的模糊PID跟蹤算法，采用GPS獲取車輛位置信息，并與目標(biāo)路徑比較，從而求出位移的偏差和車輛航向角的偏差，以此作為控制器的輸入信號，根據(jù)偏差量和偏差量的變化程度來決定方向盤的轉(zhuǎn)向和車速，運用補償控制對模糊控制器的輸出值進(jìn)行糾正，從而控制無人駕駛車輛實現(xiàn)路徑的準(zhǔn)確跟蹤。利用圖像作為信息采集，對舵機(jī)采用PD控制算法，根據(jù)舵機(jī)轉(zhuǎn)向角設(shè)定小車的速度，并對小車實行轉(zhuǎn)角和速度的實時控制。一、路徑跟蹤基于當(dāng)前偏差的路徑跟蹤算法示意

由(Xi,Yi)與(Xi+1,Yi+1)構(gòu)成的是規(guī)劃的理想的行走路線。當(dāng)前車的中心點位于(x,y)處，其α、β、γ、θ逆時針方向為正。α為實際航向與當(dāng)前期望航向的夾角；β為INS輸出的當(dāng)前航向；γ為當(dāng)前期望航向；φ為期望角。L為無人車與目標(biāo)點的直線距離；S為無人車與路徑的垂直距離。一、路徑跟蹤基于當(dāng)前偏差的路徑跟蹤算法示意其中，同理，可推斷其他位置時的狀態(tài)變量。

控制變量為α、θ、L、S，控制策略為使α、θ、L、S四個量盡可能最小最優(yōu)。由控制器解算出四個量后，運用算法判斷無人車位置，然后發(fā)命令給控制機(jī)構(gòu)執(zhí)行命令。一、路徑跟蹤基于未來預(yù)測的路徑跟蹤技術(shù)通過期望路徑產(chǎn)生描述車輛運動的期望動力學(xué)物理量，然后通過車輛狀態(tài)反饋進(jìn)行跟蹤控制。這類控制方法主要是根據(jù)期望路徑計算出描述車輛跟蹤目標(biāo)路徑的車輛自身物理量，如車輛橫擺角速度、側(cè)向加速度等，然后設(shè)計反饋控制系統(tǒng)來跟蹤這些物理量。一、路徑跟蹤速度PID控制器設(shè)計通過控制器設(shè)計規(guī)則①剎車與油門分別用一套增量式PID控制器。②油門與剎車不能同時控制，所以要加入互斥原理，否則將產(chǎn)生嚴(yán)重后果。③為了保證速度的穩(wěn)定性和車身零部件的壽命，油門與剎車切換不能太頻繁。④當(dāng)當(dāng)前速度大于命令速度一定值或者剎車鎖有效的時候進(jìn)入剎車控制，當(dāng)超過命令速度一定時限，釋放油門。然后進(jìn)入剎車PID控制算法。當(dāng)命令速度與當(dāng)前速度之差在一定范圍內(nèi)的時候，或者當(dāng)前速度很小的時候，剎車控制鎖不再有效。⑤剎車控制鎖無效的時候，進(jìn)入油門控制，判斷剎車控制標(biāo)志位是否有效，有效則釋放剎車，將油門控制標(biāo)志位置位。當(dāng)當(dāng)前速度大于命令速度一定值，則將停止油門控制標(biāo)志位置位，否則停止油門標(biāo)志控制位清0。然后根據(jù)停止油門標(biāo)志位進(jìn)入油門PID控制算法控制油門。一、路徑跟蹤純追蹤算法原理①剎車與純追蹤模型基于幾何原理，根據(jù)轉(zhuǎn)角和轉(zhuǎn)彎半徑的關(guān)系，己知車輛目標(biāo)點和當(dāng)前點，這可以規(guī)劃出一條圓弧，從而計算出轉(zhuǎn)角。其模型和原理簡單、效果優(yōu)良，純追蹤控制算法已經(jīng)廣泛用到地面機(jī)器人的路徑跟蹤控制，最近在無人機(jī)也開始得到應(yīng)用。只考慮車體在二維平面上的運動。純追蹤模型如圖所示。假設(shè)圖中X,Y軸構(gòu)成了機(jī)體坐標(biāo)系，點(x,y)是目標(biāo)點，為規(guī)劃路徑上的一點，Lfw為車體坐標(biāo)系和點(x,y)弧段的弦長,也為前視距離。R則是弧段的曲率半徑。用于轉(zhuǎn)向的模糊-純追蹤控制器設(shè)計一、路徑跟蹤用于轉(zhuǎn)向的模糊-純追蹤控制器設(shè)計

由幾何模型可得如下公式推導(dǎo)：上式中，L為無人駕駛車輛前后軸距，為定值。只要x和Lfw兩者一旦確定，那么轉(zhuǎn)向角也就確定下來，給方向控制器規(guī)劃提供了理論基礎(chǔ)。同時從中也可以看出，前視距離Lfw的選擇直接影響整個轉(zhuǎn)向過程的精度。一、路徑跟蹤用于轉(zhuǎn)向的模糊-純追蹤控制器設(shè)計

由幾何模型可得如下公式推導(dǎo)：上式中，L為無人駕駛車輛前后軸距，為定值。只要x和Lfw兩者一旦確定，那么轉(zhuǎn)向角也就確定下來，給方向控制器規(guī)劃提供了理論基礎(chǔ)。同時從中也可以看出，前視距離Lfw的選擇直接影響整個轉(zhuǎn)向過程的精度。一、路徑跟蹤模糊控制概述剎車目前的控制系統(tǒng)不再是線性或者僅僅是一個簡單的模型就能夠形容，更多的大規(guī)模復(fù)雜系統(tǒng)而是具有多變量、非線性、時變等特點，要想建立精準(zhǔn)的數(shù)學(xué)模型幾乎不可能。工業(yè)現(xiàn)實要求的嚴(yán)酷對系統(tǒng)的精準(zhǔn)性提出了高標(biāo)準(zhǔn)的要求，然而系統(tǒng)的建模又非常困難，現(xiàn)有的經(jīng)典控制理論方法就不能再滿足控制系統(tǒng)高性能的需要。模糊控制，是一種基于模糊集與推理規(guī)則的控制理論，是近代智能控制理論的一個重要組成部分。模糊控制相比其它控制理論方法，具有一些特有的特點：它把人類專家的控制經(jīng)驗和知識表示成語言控制規(guī)則，更接近人的思維方法和推理習(xí)慣；可以處理模糊性的信息且不依賴于被控對象的數(shù)學(xué)模型；同時其具有適應(yīng)性。

轉(zhuǎn)向的模糊-純追蹤控制器設(shè)計一、路徑跟蹤模糊控制對于解決數(shù)學(xué)模型未知或者復(fù)雜方面的問題提供了一種良好的解決辦法。汽車本身就是一個剛體非線性時變系統(tǒng)，而且受到車輛行駛時各種復(fù)雜情況的影響，傳統(tǒng)的控制方法不能適應(yīng)復(fù)雜的變化，建立模型困難。方向控制器就采用模糊控制來實現(xiàn)。轉(zhuǎn)向的模糊-純追蹤控制器設(shè)計一、路徑跟蹤模糊控制概述剎車目前的控制系統(tǒng)不再是線性或者僅僅是一個簡單的模型就能夠形容，更多的大規(guī)模復(fù)雜系統(tǒng)而是具有多變量、非線性、時變等特點，要想建立精準(zhǔn)的數(shù)學(xué)模型幾乎不可能。工業(yè)現(xiàn)實要求的嚴(yán)酷對系統(tǒng)的精準(zhǔn)性提出了高標(biāo)準(zhǔn)的要求，然而系統(tǒng)的建模又非常困難，現(xiàn)有的經(jīng)典控制理論方法就不能再滿足控制系統(tǒng)高性能的需要。模糊控制，是一種基于模糊集與推理規(guī)則的控制理論，是近代智能控制理論的一個重要組成部分。模糊控制相比其它控制理論方法，具有一些特有的特點：它把人類專家的控制經(jīng)驗和知識表示成語言控制規(guī)則，更接近人的思維方法和推理習(xí)慣；可以處理模糊性的信息且不依賴于被控對象的數(shù)學(xué)模型；同時其具有適應(yīng)性。模糊控制對于解決數(shù)學(xué)模型未知或者復(fù)雜方面的問題提供了一種良好的解決辦法。轉(zhuǎn)向的模糊-純追蹤控制器設(shè)計一、路徑跟蹤模糊化模糊分割數(shù)決定模糊規(guī)則最大可能的數(shù)目，所以模糊分割不建議太細(xì)，否則就增加了計算的復(fù)雜度以及控制規(guī)則的復(fù)雜度；同時分割數(shù)過少導(dǎo)致分割太粗，控制精度受到極大影響。因此綜合兩者考慮，一般對模糊分割數(shù)分為7擋。無論如何前視距離在任何情況下應(yīng)該大于1m才能保證系統(tǒng)的穩(wěn)定性，同時推薦前視距離取當(dāng)前速度的2~3s倍，如此的跟蹤效果最好。規(guī)則表規(guī)則根據(jù)對人類在實際駕駛過程中對前視距離的選擇經(jīng)驗總結(jié)而來，模糊控制規(guī)則表是模糊控制器的關(guān)鍵步驟，它直接決定著控制器的邏輯關(guān)系，從而影響控制器的性能。去模糊化考慮到實時性和算法的復(fù)雜性，去模糊化用重心法。轉(zhuǎn)向的模糊-純追蹤控制器設(shè)計一、路徑跟蹤在無人駕駛車輛方向控制過程中，前輪駕駛角命令δ0（在無人駕駛車輛中其對應(yīng)一定的轉(zhuǎn)向電機(jī)脈沖）與車輛前輪實際轉(zhuǎn)向角度的關(guān)系十分重要。誤差原因包括車輛轉(zhuǎn)向機(jī)構(gòu)故有的誤差、測量過程中的誤差等，但是根據(jù)測試結(jié)果，誤差還是比較小的。當(dāng)向左轉(zhuǎn)的時候，編碼器的值是右前輪的值；而向右轉(zhuǎn)的時候，編碼器的值是左前輪的值。車輛前輪轉(zhuǎn)向角標(biāo)定綜合實驗驗證目前幾種常見的路徑規(guī)劃結(jié)果包括：直行、類sin曲線、弧線、左圓弧、右圓弧、U型路徑。二、PID控制算式的數(shù)字化

PID系統(tǒng)原理

模擬PID控制算式二、PID控制算式的數(shù)字化模擬PID控制算式PID控制算式公式中的積分和微分項不能直接使用，必須進(jìn)行離散化處理。二、PID控制算式的數(shù)字化PID控制算式的離散化離散化處理的方法為：以T作為采樣周期，k作為采樣序號，則離散采樣時間kT對應(yīng)著連續(xù)時間t，用求和的形式代替積分，用增量的形式代替微分，可作如下近似變換：二、PID控制算式的數(shù)字化離散的PID表達(dá)式為了表示方便，將類似于e（kT）簡化成ek形式就可以得到離散的PID表達(dá)式：或?qū)懗桑憾?、PID控制算式的數(shù)字化編程時的PID算式在編程時，可寫成：Uo(n)=P*e(n)+I*[e(n)+e(n-1)+...+e(0)]+D*[e(n)-e(n-1)]P：改變P可提高響應(yīng)速度，減小靜態(tài)誤差，但太大會增大超調(diào)量和穩(wěn)定時間。I：與P的作用基本相似，但要使靜態(tài)誤差為0，必須使用積分。D：與P,I的作用相反，主要是為了減小超調(diào)，減小穩(wěn)定時間。e(n)：本次誤差e(n)+e(n-1)+...+e(0)：所有誤差之和e(n)-e(n-1)：控制器輸出與輸入誤差信號的微分（即誤差的變化率）二、PID控制算式的數(shù)字化增量式PID算法式中：式中：T為采樣周期一般來說，控制系統(tǒng)的采樣周期T一經(jīng)選擇后就不會再有所改變，因此在上式中，KP、TD、TI三個參數(shù)確定后，只需要經(jīng)過3次測量偏差值即可求出控制增量，方便簡單。二、PID控制算式的數(shù)字化參數(shù)調(diào)試方法

三個參數(shù)要綜合考慮，一般先將I，D設(shè)為0，調(diào)好P，達(dá)到基本的響應(yīng)速度和誤差，再加上I，使誤差為0，這時再加入D，三個參數(shù)要反復(fù)調(diào)試，最終達(dá)到較好的結(jié)果。不同的控制對象，調(diào)試的難度相差很大！

在實際的應(yīng)用中，更多的是通過湊試法來確定PID的參數(shù)。增大比例系數(shù)P一般將加快系統(tǒng)的響應(yīng)，在有靜差的情況下有利于減小靜差，但是過大的比例系數(shù)會使系統(tǒng)有比較大的超調(diào)，并產(chǎn)生振蕩，使穩(wěn)定性變壞。增大積分時間I有利于減小超調(diào)，減小振蕩，使系統(tǒng)的穩(wěn)定性增加，但是系統(tǒng)靜差消除時間變長。增大微分時間D有利于加快系統(tǒng)的響應(yīng)速度，使系統(tǒng)超調(diào)量減小，穩(wěn)定性增加，但系統(tǒng)對擾動的抑制能力減弱。二、PID控制算式的數(shù)字化參數(shù)調(diào)試方法微分是即誤差的變化率，具有預(yù)見性，能預(yù)見偏差變化的趨勢，因此能產(chǎn)生超前的控制作用，在偏差還沒有形成之前，已被微分調(diào)節(jié)作用消除。因此，可以改善系統(tǒng)的動態(tài)性能。在微分時間選擇合適情況下，可以減少超調(diào)，減少調(diào)節(jié)時間。微分作用對噪聲干擾有放大作用，因此過強(qiáng)的加微分調(diào)節(jié)，對系統(tǒng)抗干擾不利。三、卡爾曼濾波與擴(kuò)展卡爾曼濾波EKF卡爾曼濾波卡爾曼濾波是一種高效率的遞歸濾波器(自回歸濾波器),它能夠從一系列完全包含噪聲的測量中，估計動態(tài)系統(tǒng)的狀態(tài)。采用信號與噪聲的狀態(tài)空間模型，利用前一時刻的估計值和現(xiàn)時刻的觀測值來更新對狀態(tài)變量的估計，求出現(xiàn)在時刻的估計值。它適合于實時處理和計算機(jī)運算。并由量測值重構(gòu)系統(tǒng)的狀態(tài)向量。它以“預(yù)測—實測—修正”的順序遞推，根據(jù)量測值來消除隨機(jī)干擾，再現(xiàn)系統(tǒng)的狀態(tài)?？柭鼮V波器的兩個重要假設(shè)：被建模的系統(tǒng)是線性的：K時刻的系統(tǒng)狀態(tài)可以用某個矩陣與K-1時刻的系統(tǒng)狀態(tài)的乘積表示。影響測量的噪聲屬于高斯分布的白噪聲：噪聲與時間不相關(guān)，且只用均值和協(xié)方差就可以準(zhǔn)確地建模。三、卡爾曼濾波與擴(kuò)展卡爾曼濾波EKF卡爾曼濾波數(shù)學(xué)推導(dǎo)