湯 巍 王冠凌

（1.安徽工程大學(xué)電氣工程學(xué)院；2.電氣傳動(dòng)與控制安徽省重點(diǎn)實(shí)驗(yàn)室 安徽蕪湖 241000）

一、引言

同時(shí)定位與地圖構(gòu)建(simultaneous localization and mapping,SLAM)是當(dāng)今研究無人駕駛問題的熱點(diǎn)，特別是在智能小車當(dāng)中。無人車在遇到一個(gè)陌生的環(huán)境時(shí)第一個(gè)問題就是“我在哪”的問題，這就需要構(gòu)圖和定位。如果在一個(gè)陌生的環(huán)境中智能車（移動(dòng)機(jī)器人）想要完成特定的任務(wù)，定位與地圖構(gòu)建的問題是首先需要解決的問題。這是一個(gè)“先有雞還是先有蛋”的問題，移動(dòng)機(jī)器人想要獲得精準(zhǔn)的自身定位信息則必須知道自身的所處環(huán)境[1]。然而想獲得陌生的環(huán)境，前提是知道移動(dòng)機(jī)器人自身的準(zhǔn)確定位信息。針對(duì)這個(gè)問題，研究者提出了SLAM技術(shù)加以解決。

傳感器是SLAM的核心部件。目前主流的傳感器是視覺傳感器和激光雷達(dá)傳感器。視覺傳感器有三種：?jiǎn)文?、雙目相機(jī)和深度相機(jī)。單目相機(jī)結(jié)構(gòu)特別簡(jiǎn)單，成本特別便宜。單目相機(jī)拍攝的圖像僅僅是三維空間的二維投影，所以無法憑借圖像確定事物的真實(shí)尺度，具有尺度不確定性。由于單目相機(jī)無法通過圖像確定其深度。所以人們?yōu)榱说玫狡渖疃乳_始使用雙目和深度相機(jī)。使用雙目相機(jī)和深度相機(jī)的目的是通過一些手段測(cè)定物體和相機(jī)之間的距離，克服單目相機(jī)無法知道距離的缺點(diǎn)。知道了距離也就消除了尺度不確定性。深度相機(jī)和雙目相機(jī)類似但也有所區(qū)別，因?yàn)樯疃认鄼C(jī)是通過物理測(cè)量手段，而雙目相機(jī)卻是通過軟件計(jì)算來解決，所以深度相機(jī)相比于雙目相機(jī)可以節(jié)省大量的計(jì)算。但是這些視覺傳感器還存在測(cè)量范圍狹小、噪聲大、視野小、易受太陽(yáng)光干擾、無法測(cè)量透視材料的等諸多問題[2]。所以在SLAM中激光雷達(dá)傳感器有更大的適用范圍。相比于視覺傳感器，激光雷達(dá)則不存在這些缺陷，對(duì)SLAM的應(yīng)用是最好的選擇。

本文智能車的同時(shí)定位與地圖構(gòu)建是用激光雷達(dá)作為外部傳感器，并且用其采集環(huán)境信息生成環(huán)境地圖；最后采用姿態(tài)位置估計(jì)算法實(shí)現(xiàn)對(duì)小車自身姿態(tài)和環(huán)境路標(biāo)的位置估計(jì)?，F(xiàn)階段SLAM實(shí)現(xiàn)方式中穩(wěn)定性可靠性好并且性能優(yōu)秀的就是基于激光雷達(dá)的定位。為了解決移動(dòng)機(jī)器人定位和地圖的問題，最早的解決方案是卡爾曼濾波的SLAM。但是這種算法有著明顯的缺點(diǎn)：算法的計(jì)算量過大，最后出來的估計(jì)精度也不盡人意。為了解決卡爾曼濾波的缺點(diǎn)，將OPTICS(ordering points to identify the clustering structure)和局部兼容數(shù)據(jù)關(guān)聯(lián)的方法解決SLAM中的數(shù)據(jù)關(guān)聯(lián)計(jì)算量大的問題;對(duì)數(shù)據(jù)關(guān)聯(lián)后相關(guān)矩陣采用自適應(yīng)漸消擴(kuò)展卡爾曼濾波[3](adaptive fading extended kalman filter,AFEKF)用來替代傳統(tǒng)卡爾曼濾波對(duì)智能車自身姿態(tài)和環(huán)境地圖更新,使小車SLAM系統(tǒng)得到精度更高的自身位置和環(huán)境地圖;最后使用MATLAB進(jìn)行軟件仿真實(shí)驗(yàn)驗(yàn)證了新型算法的可靠性。

（一）SLAM問題和數(shù)學(xué)表述。

1.SLAM問題表述。即時(shí)定位與地圖構(gòu)建的問題最通俗的說法是：移動(dòng)機(jī)器人從未知環(huán)境中的未知初始點(diǎn)出發(fā)，移動(dòng)機(jī)器人使用激光雷達(dá)傳感器感知周圍環(huán)境，后根據(jù)小車自身的激光雷達(dá)傳感器獲取的外界環(huán)境信息構(gòu)建初始的環(huán)境地圖，然后隨著小車的移動(dòng)對(duì)自身姿態(tài)和環(huán)境信息進(jìn)行更新，這個(gè)問題是一個(gè)循環(huán)問題十分類似與“雞與蛋”的問題。SLAM的模型如圖所示，我們需要智能車和標(biāo)志物的位置，真實(shí)的位置是沒有辦法被直接測(cè)量的，觀察是相對(duì)于智能車和標(biāo)志物之間的。

SLAM算法問題模型

2.SLAM數(shù)學(xué)描述。智能車所接受的控制指令和激光雷達(dá)所觀測(cè)的外界環(huán)境是智能小車中主要的輸入?yún)?shù)。其中控制指令有小車行駛的速度和車輪的偏轉(zhuǎn)角。觀測(cè)信息是周圍陌生的環(huán)境信息。最終小車在行駛過程中所得到的輸出信息有：小車的行駛軌跡圖。SLAM在概率學(xué)的形式中，服從以下概率分布：

該概率分布描述了在已知的觀察和控制輸入情況下，輪式移動(dòng)機(jī)器人的位置和隨即在k時(shí)刻的狀態(tài)的聯(lián)合后驗(yàn)分布密度[4]。通常我們要用遞歸來解決這個(gè)問題。我們從時(shí)刻為k-1時(shí)的聯(lián)合后驗(yàn)分布密度P(xk-1,m|Z0:k-1,U0:k-1,x0)開始計(jì)算，隨后通過貝葉斯理論計(jì)算出控制量uk和觀察量zk，最終算出這個(gè)密度。但是上述計(jì)算需要考慮出一個(gè)狀態(tài)轉(zhuǎn)移模型和觀察模型用以描述控制輸入和觀察量的影響。其中觀察模型描述了當(dāng)機(jī)器人的位置和標(biāo)志物的位置都在已知的情況時(shí)，做出觀察zk的概率，我們通常把這個(gè)概率寫作：

一旦我們將智能車的位置和地圖都定義好，接下來我們就假設(shè)不同時(shí)刻的觀察之間是相對(duì)條件獨(dú)立的。此時(shí)可以將小車的運(yùn)動(dòng)模型用狀態(tài)轉(zhuǎn)換的概率分布來描述。

我們將狀態(tài)轉(zhuǎn)換概率假設(shè)為一個(gè)馬爾科夫鏈，在這馬爾科夫鏈過程中，下一時(shí)刻的狀態(tài)xk的決定只由上一時(shí)刻xk-1和控制量uk所決定，而且這個(gè)狀態(tài)的轉(zhuǎn)換獨(dú)立于觀察和測(cè)繪的過程。接下來SLAM算法可以通過標(biāo)準(zhǔn)的兩步遞歸計(jì)算出相關(guān)度（按時(shí)間順序更新，按測(cè)量更新）更新的形式[1]：

Time-update:按時(shí)間更新

Measurement Update:按測(cè)量更新

計(jì)算聯(lián)合后驗(yàn)分布密度P(xk,m|Z0:k,U0:k,x0)的迭代步驟如上述方程4和方程5所示。地圖構(gòu)建問題可以等效為計(jì)算概率密度P(m|X0:k,Z0:k,U0:k)的問題，這種等效假設(shè)小車的位置xk在所有時(shí)刻都是已知的，這樣我們就可以融合地圖中每個(gè)時(shí)刻的不同位置就可以確定小車的位置。同樣的定位問題也可以等效看為計(jì)算概率密度p(xk|z0:k,U0:k,m)的問題,類似，為了計(jì)算小車的位置，這種等效要假設(shè)標(biāo)志物的位置是已知的。

二、小車系統(tǒng)設(shè)計(jì)原理

智能車通過激光雷達(dá)傳感器并且用改進(jìn)的卡爾曼濾波算法和快速連續(xù)兼容分支定界關(guān)聯(lián)算法作為智能車SLAM系統(tǒng)。系統(tǒng)采用快速聯(lián)合兼容支定界關(guān)聯(lián)算法[3](Joint Compatibility Branch and Bound，JCBB)，該算法比原算法獲得的關(guān)聯(lián)結(jié)果更加精確；對(duì)小車的姿態(tài)和環(huán)境地圖的估計(jì)實(shí)現(xiàn)采用自適應(yīng)漸消擴(kuò)展卡爾曼濾波?；诩す饫走_(dá)的AFEKF-SLAM系統(tǒng)主體框架如下圖所示:

AFEKF-SLAM系統(tǒng)主體框架

（一）局部關(guān)聯(lián)數(shù)據(jù)和OPTICS改進(jìn)的數(shù)據(jù)融合算法。傳感器數(shù)據(jù)的融合技術(shù)上實(shí)現(xiàn)困難度很大，并且這個(gè)問題一直是SLAM問題的重點(diǎn)和難點(diǎn)。它是現(xiàn)今無人駕駛技術(shù)提高精確度的重要方法。如今，和SLAM有關(guān)的數(shù)據(jù)融合算法主要是對(duì)參數(shù)量測(cè)和環(huán)境特征融合關(guān)聯(lián)的方法如聯(lián)合兼分枝定界（JCBB)算法[3];JCBB數(shù)據(jù)融合準(zhǔn)確度高，但計(jì)算過程復(fù)雜，響應(yīng)時(shí)間長(zhǎng)。若智能車處在環(huán)境復(fù)雜的自然環(huán)境中，那么融合數(shù)據(jù)所需要的計(jì)算量是十分龐大的，即便如此JCBB算法仍然是不可或缺的優(yōu)秀算法。

JCBB[5]算法相比較其他算法的優(yōu)點(diǎn)是兼容了幾乎所有觀測(cè)與地圖特征點(diǎn)，并且為了搜索兼容性最大的關(guān)聯(lián)假設(shè)采用了分支定界法，其本質(zhì)上是針對(duì)單變量對(duì)環(huán)境進(jìn)行圖搜索[6]，將所得到的環(huán)境信息進(jìn)行融合。且其總體復(fù)雜度為)；除此之外，當(dāng)數(shù)據(jù)進(jìn)行融合并開始計(jì)算時(shí)，環(huán)境信息的計(jì)算量都與激光雷達(dá)的觀測(cè)信息都與m相關(guān)，其中最為主要的是激光雷達(dá)距離標(biāo)志點(diǎn)的距離[7]在計(jì)算中需要對(duì)剛更新的地圖信息求逆運(yùn)算，計(jì)算復(fù)雜度為O(m3)。所以JCBB算法的時(shí)間復(fù)雜度受：傳感器觀測(cè)數(shù)和已更新地圖中特征點(diǎn)個(gè)數(shù)的影響。減少融合后環(huán)境地圖特征點(diǎn)數(shù)和傳感器觀測(cè)點(diǎn)數(shù)可以有效的降低其空間算法復(fù)雜度，減少硬件資源的浪費(fèi)，和降低以后使用的成本。所以可以對(duì)JCBB算法進(jìn)行以下的改進(jìn)：一是采用局部數(shù)據(jù)融合；二是采用OPTICS法對(duì)觀測(cè)數(shù)和地圖特征數(shù)進(jìn)行聚類[3]。

1.采用局部數(shù)據(jù)融合。將需要融合的數(shù)據(jù)在地圖中的局部點(diǎn)進(jìn)行融合，這樣能有效減少融合地圖的特征點(diǎn)數(shù)，降低融合計(jì)算時(shí)的計(jì)算量，融合后的地圖的局部計(jì)算復(fù)雜度為O(mn)。定義局部地圖可用如下公式表示:

式中，(xv,yv,θ)表示當(dāng)前時(shí)刻小車所在環(huán)境的三維坐標(biāo)，(xi,yi)表示地圖中標(biāo)志物的平面坐標(biāo)，r為傳感器達(dá)最大的作用范圍，Δd為范圍補(bǔ)償距離。

2.基于OPTICS的分組數(shù)據(jù)融合。因?yàn)閭鹘y(tǒng)JCBB算法需要計(jì)算整張環(huán)境地圖與傳感器采集的特征點(diǎn)數(shù)據(jù)融合[6]，所以環(huán)境的復(fù)雜度會(huì)與JCBB算法的計(jì)算量成正相關(guān)，并且計(jì)算量的增加會(huì)根據(jù)環(huán)境特征點(diǎn)的增加呈指數(shù)增長(zhǎng)。因此考慮到智能車行駛在自然環(huán)境中，激光雷達(dá)傳感器所觀測(cè)到的特征值有顯著差異，故將JCBB數(shù)據(jù)融合開始前，首先采用密度聚類方法對(duì)當(dāng)前傳感器輸出的觀測(cè)值進(jìn)行分組聚類，進(jìn)行分組聚類后的數(shù)據(jù)得到若干數(shù)據(jù)組；其次在每個(gè)數(shù)據(jù)組中采用JCBB數(shù)據(jù)融合算法，這樣就能得到許多組數(shù)據(jù)融合組解；最后，將得到的數(shù)據(jù)融合解組獲得的數(shù)據(jù)再進(jìn)行融合，最終得到最優(yōu)的數(shù)據(jù)融合組Hbest。

（二）基于AFEKF的小車狀態(tài)和特征地圖估計(jì)。傳統(tǒng)的卡爾曼濾波SLAM有著一個(gè)缺陷:移動(dòng)小車運(yùn)動(dòng)模型具有很強(qiáng)的非線性，對(duì)環(huán)境的影響是分敏感，若將該小車系統(tǒng)模型進(jìn)行相應(yīng)的簡(jiǎn)化，這樣會(huì)導(dǎo)致傳感器所采集的環(huán)境信息準(zhǔn)確度下降，與此同時(shí)對(duì)EKF-SLAM系統(tǒng)的魯棒性有很大影響，濾波并不會(huì)收斂。模型簡(jiǎn)化會(huì)使得EKF－SLAM的小車自身姿態(tài)和環(huán)境構(gòu)建精度下降和收斂速度下降[7]。為了解決精確度低和收斂速度速度過慢的問題，采用一種改進(jìn)的擴(kuò)展卡爾曼濾波器SLAM算法，簡(jiǎn)稱AFEKF－SLAM。它對(duì)小車系統(tǒng)采用AFEKF－SLAM的方法對(duì)環(huán)境地圖進(jìn)行實(shí)時(shí)更新，并通過漸消因子對(duì)地圖進(jìn)行有效補(bǔ)償，有效改進(jìn)了非線性系統(tǒng)的復(fù)雜度和小車行駛過程中的參數(shù)漂移對(duì)狀態(tài)估計(jì)造成的影響，使得SLAM結(jié)果的準(zhǔn)確度有所提高。

智能小車的運(yùn)動(dòng)模型和激光雷達(dá)掃描模型數(shù)學(xué)表達(dá)式分別為[8]:

其中，xv,t是智能車在t時(shí)刻的三維姿態(tài)，zj,t是激光雷達(dá)傳感器在t時(shí)刻的距離和角度信息。L為車軸距離，v和G為t時(shí)刻系統(tǒng)控制輸入量，dt表示輸入量采樣所需要的時(shí)間，v表示小車行駛速度，G是小車水平方向的夾角。(xj,yj)是激光雷所采集到環(huán)境第j個(gè)特征點(diǎn)的坐標(biāo)[9]。

在小車運(yùn)動(dòng)系統(tǒng)中，可以將小車系統(tǒng)的狀態(tài)向量x設(shè)置為:

其中，xv表示智能車這個(gè)時(shí)刻的狀態(tài)向量，xm代表此時(shí)更新后的環(huán)境地圖的向量。wt和vt分別表示為采集外界信息和觀測(cè)信息，它們服從高斯分布，該高斯分布均值為0，方差為Q。

1.狀態(tài)預(yù)測(cè)。根據(jù)智能車在運(yùn)動(dòng)過程中t-1時(shí)刻系統(tǒng)的運(yùn)動(dòng)狀態(tài)xt-1、狀態(tài)協(xié)方差pt-1來預(yù)測(cè)下一個(gè)t時(shí)刻的系統(tǒng)狀態(tài)和狀態(tài)協(xié)方差pt-1|t，具體計(jì)算步驟是:

其中xt-1=[xt-1,yt-1,θt-1]T，GV和Gu為雅克比矩陣。

2.數(shù)據(jù)融合。基于前文提出的融合數(shù)據(jù)的方法得到最優(yōu)的數(shù)據(jù)融合解組[10]，將當(dāng)前時(shí)刻的觀測(cè)量zt分為對(duì)應(yīng)地圖特征值的采集點(diǎn)zft和新的環(huán)境采集點(diǎn)znt，這兩個(gè)采集點(diǎn)可以分別用來完成小車自身狀態(tài)的更新和地圖增廣。

3.狀態(tài)更新。根據(jù)觀測(cè)到的小車信息zft對(duì)智能車的自身姿態(tài)和采集到的環(huán)境地圖特征值進(jìn)行實(shí)時(shí)更新。首先根據(jù)公式(7)中的觀測(cè)模型獲得觀測(cè)的均值z(mì)?t[11]，并對(duì)激光雷達(dá)傳感器的觀測(cè)量zft和觀測(cè)均指z?t進(jìn)行比較，可以獲得更新后的信息及地圖的協(xié)方差，公式如下:

上述公式EKF中引入了自適應(yīng)漸消因子λt，能夠更好的預(yù)測(cè)誤差，同時(shí)也將漸消因子引入?yún)f(xié)方差中，用增益矩陣進(jìn)行在線補(bǔ)償，詳細(xì)公式如下所示:

上述公式中其中，Pv為小車狀態(tài)向量協(xié)方差矩陣，Pm代表地圖向量協(xié)方差矩陣，Pvm表示車輛運(yùn)動(dòng)狀態(tài)向量及地圖向量的互協(xié)方差矩陣。漸消因子具體計(jì)算步驟如下所示:

4.狀態(tài)增廣。如果小車在行駛的過程中沒有觀測(cè)到新的標(biāo)志物[8]，則不需要進(jìn)行狀態(tài)增廣; 如果當(dāng)前時(shí)刻激光雷達(dá)觀測(cè)到了新的觀測(cè)量znt，那么對(duì)此時(shí)小車進(jìn)行狀態(tài)增廣，數(shù)學(xué)處理步驟如下:

第一步小車運(yùn)動(dòng)過程中沒有進(jìn)行狀態(tài)增廣:

其中，znt=[r?]T,r為激光雷達(dá)采集到的信息到此時(shí)智能車的距離,?為標(biāo)志物與小車此時(shí)方向的夾角。xt=[xyθM]T,(xaug,t,yaug,t)為新觀測(cè)到的特征點(diǎn)坐標(biāo)，其計(jì)算公式如下:

狀態(tài)協(xié)方差增廣為:

三、實(shí)驗(yàn)分析

本文用matlab對(duì)提出的改進(jìn)卡爾曼濾波算法進(jìn)行matlab仿真，并且將該算法系統(tǒng)得到的實(shí)驗(yàn)結(jié)果與傳統(tǒng)JCBB算法融合方法的SLAM系統(tǒng)結(jié)果進(jìn)行比較，說明改進(jìn)算法的優(yōu)點(diǎn)。

本文給出仿真實(shí)驗(yàn)環(huán)境如下所示，如圖1所示。其中紅色星號(hào)表示實(shí)驗(yàn)前設(shè)置的已知標(biāo)志物的具體位置，藍(lán)色星號(hào)表示利用AFEKF-SLAM算法計(jì)算出小車可能的具體位置，圖中的行動(dòng)軌跡表示智能小車的預(yù)先設(shè)定的路徑。圖中總共設(shè)定了11個(gè)地標(biāo)和24個(gè)路標(biāo)，傳統(tǒng)的JCBB-SLAM算法與該機(jī)的擴(kuò)展卡爾曼濾波（AFEKF）和局部數(shù)據(jù)融合移動(dòng)機(jī)器人AFEKF-SLAM算法結(jié)果進(jìn)行比較。

圖1 移動(dòng)機(jī)器人仿真環(huán)境

圖2 實(shí)驗(yàn)路徑跟蹤圖

圖3 X軸誤差比較

圖4 Y軸誤差比較

由上圖的仿真結(jié)果可以看出，融合自適應(yīng)漸消擴(kuò)展卡爾曼濾波（AFEKF）和數(shù)據(jù)融合關(guān)聯(lián)的方法與傳統(tǒng)SLAM算法相比有更大的優(yōu)勢(shì)，其運(yùn)動(dòng)軌跡路徑與智能小車真實(shí)的路徑重合率更好，即預(yù)計(jì)的路標(biāo)點(diǎn)和智能小車的位姿更加接近真實(shí)的情況和智能車的位姿，創(chuàng)建的地圖也更加精確。由圖3圖4可知，本文提出算法在橫坐標(biāo)和縱坐標(biāo)方向的估計(jì)誤差都優(yōu)于JCBB-SLAM算法。AFEKF-SLAM算法比JCBBSLAM算法估計(jì)的狀態(tài)更加接近于真實(shí)值，從而提高了移動(dòng)機(jī)器人的位姿估計(jì)精度。

四、結(jié)論

SLAM技術(shù)是智能車以及以后無人駕駛技術(shù)實(shí)現(xiàn)的重點(diǎn)和基礎(chǔ)，本文設(shè)計(jì)了基于激光雷達(dá)的AFEKF－SLAM系統(tǒng)。AFEKF－SLAM系統(tǒng)采用改進(jìn)的擴(kuò)展卡爾曼濾波數(shù)據(jù)融合的方法是小車行駛的精確度更好。并且將漸消因子引入EKF中，對(duì)小車的位姿和環(huán)境信息的位置進(jìn)行更加準(zhǔn)確的估計(jì)。并對(duì)兩種方法進(jìn)行仿真檢測(cè)。計(jì)算表明AFEKFSLAM系統(tǒng)復(fù)雜度低，運(yùn)行效率高，且仿真結(jié)果表明AFEKF－SLAM精度要由于傳統(tǒng)的JCBB-SLAM，其能夠在不同復(fù)雜環(huán)境中為移動(dòng)小車SLAM的實(shí)時(shí)性和準(zhǔn)確性提供可靠的保證。