基于卡爾曼濾波器的自動駕駛算法

組成自動駕駛的關(guān)鍵技術(shù)包括感知、規(guī)劃和控制三大部分。自動駕駛車通過傳感器感知環(huán)境并進(jìn)行定位，根據(jù)感知系統(tǒng)獲得的信息和行駛目標(biāo)進(jìn)行速度和路徑的規(guī)劃，并以控制系統(tǒng)為基礎(chǔ)實現(xiàn)車輛的運動控制。

背景

導(dǎo)航是感知系統(tǒng)下重要的模塊，其性能優(yōu)越程度決定了自動駕駛的性能?；跀U(kuò)展卡爾曼濾波器的GNSS/SINS松耦合組合是最常見的導(dǎo)航算法之一，但是此算法對GNSS的魯棒性極低，不適用于狹窄的道路。另一個重要的方法是車道檢測和跟蹤。一種是基于霍夫變換表示的視覺，但這種方法要求路面高度清潔。另一種基于激光雷達(dá)，通過獲取雷達(dá)掃描點的坐標(biāo)并將其轉(zhuǎn)換為光柵圖像，然后利用光柵圖像中的點密度提取車道線。

規(guī)劃系統(tǒng)的經(jīng)典形式是比例積分微分（PID）控制器，它不需要系統(tǒng)模型，并且基于誤差信號設(shè)計了控制率。但當(dāng)路徑的曲率較大時，跟蹤精度會降低。另一種典型的方法是基于車輛運動學(xué)模型。該方法利用車輛運動模型預(yù)測一段時間內(nèi)的未來運動，并通過連續(xù)優(yōu)化的控制參數(shù)來擬合這一系列運動，但此方法只考慮重心的偏差和預(yù)測時域中的偏差。

在這項研究中，研究人員基于平方根容積卡爾算法（SCKF），以二階固定非線性展開為測量模型，設(shè)計了融合IMU、GNSS接收機(jī)和里程表的組合導(dǎo)航控制算法。導(dǎo)航控制算法將預(yù)期路線劃分為多段，并根據(jù)電子地圖的經(jīng)緯度、航向角和關(guān)鍵點序列來進(jìn)行計算，最終得到無人駕駛車輛與分段路線之間的角度偏差。該算法通過增量式PID控制器消除偏差，實現(xiàn)逐段路徑跟蹤。仿真實驗結(jié)果表明，基于SCKF的組合導(dǎo)航算法的精度優(yōu)于基于SKF的組合導(dǎo)航。該系統(tǒng)的組成如圖1所示。

圖1

導(dǎo)航系統(tǒng)

基于慣性導(dǎo)航誤差模型的濾波器在觀測異常時容易出現(xiàn)異常甚至發(fā)散。其原因是基于小誤差假設(shè)建立了組合導(dǎo)航的濾波觀測方程。當(dāng)慣性導(dǎo)航誤差變大時，濾波觀測方程的線性化過程將不理想。

因此，在本研究中，引入了一種非線性濾波器，立方體卡爾曼濾波器（SCKF），該濾波器使用三階球面徑向體積規(guī)則來近似非線性變換中所需的積分運算。為了避免矩陣計算過程中的失真，本文推導(dǎo)了與平方根卡爾曼濾波器相似的立方卡體爾曼濾波器。

最終的推導(dǎo)結(jié)果如下，Wk為反饋矩陣，X k|k為最佳狀態(tài)，Pk|k為方差矩陣。

引導(dǎo)算法

引導(dǎo)控制算法基于地圖信息和關(guān)鍵點生成坐標(biāo)序列，并生成用于分段運動控制的引導(dǎo)參數(shù)，以根據(jù)所建立的路線實現(xiàn)自動駕駛。制導(dǎo)控制算法的體系結(jié)構(gòu)如圖3所示。

圖3

考慮到具有輪距W和車輪半徑r的四輪驅(qū)動車輛，四輪轉(zhuǎn)向模型如圖4所示。

圖4

制導(dǎo)參數(shù)由航向角偏差d和橫向距離偏差r組成。制導(dǎo)參數(shù)與關(guān)鍵點坐標(biāo)序列的關(guān)系如圖5所示。

圖5

結(jié)論

研究人員對該系統(tǒng)進(jìn)行了仿真測驗，結(jié)果表明，基于SCKF的組合導(dǎo)航算法的精度優(yōu)于基于SKF的組合導(dǎo)航。為了驗證自動駕駛算法的有效性及其對衛(wèi)星信號干擾的魯棒性，進(jìn)行了無人駕駛汽車的室外自動駕駛實驗。該算法是自動駕駛儀的基本形式，具有很高的工程應(yīng)用價值和參考價值。這項工作缺乏對環(huán)境感知的處理，例如障礙物的存在。因此，下一步的研究將集中于在線路徑規(guī)劃和重構(gòu)。