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

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

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

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

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

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

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

PID系統(tǒng)原理

模擬PID控制算式二、PID控制算式的數(shù)字化模擬PID控制算式PID控制算式公式中的積分和微分項不能直接使用，必須進行離散化處理。二、PID控制算式的數(shù)字化PID控制算式的離散化離散化處理的方法為：以T作為采樣周期，k作為采樣序號，則離散采樣時間kT對應著連續(xù)時間t，用求和的形式代替積分，用增量的形式代替微分，可作如下近似變換：二、PID控制算式的數(shù)字化離散的PID表達式為了表示方便，將類似于e（kT）簡化成ek形式就可以得到離散的PID表達式：或寫成：二、PID控制算式的數(shù)字化編程時的PID算式在編程時，可寫成：Uo(n)=P*e(n)+I*[e(n)+e(n-1)+...+e(0)]+D*[e(n)-e(n-1)]P：改變P可提高響應速度，減小靜態(tài)誤差，但太大會增大超調量和穩(wěn)定時間。I：與P的作用基本相似，但要使靜態(tài)誤差為0，必須使用積分。D：與P,I的作用相反，主要是為了減小超調，減小穩(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ù)調試方法

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

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