湯 巍 王冠凌

（1.安徽工程大學(xué)電氣工程學(xué)院；2.電氣傳動與控制安徽省重點實驗室 安徽蕪湖 241000）

一、引言

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

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

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

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

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

SLAM算法問題模型

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

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

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

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

Time-update:按時間更新

Measurement Update:按測量更新

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

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

智能車通過激光雷達(dá)傳感器并且用改進的卡爾曼濾波算法和快速連續(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é)果更加精確；對小車的姿態(tài)和環(huán)境地圖的估計實現(xiàn)采用自適應(yīng)漸消擴展卡爾曼濾波。基于激光雷達(dá)的AFEKF-SLAM系統(tǒng)主體框架如下圖所示:

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

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

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

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

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

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

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

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

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

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

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

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

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

2.數(shù)據(jù)融合?；谇拔奶岢龅娜诤蠑?shù)據(jù)的方法得到最優(yōu)的數(shù)據(jù)融合解組[10]，將當(dāng)前時刻的觀測量zt分為對應(yīng)地圖特征值的采集點zft和新的環(huán)境采集點znt，這兩個采集點可以分別用來完成小車自身狀態(tài)的更新和地圖增廣。

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

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

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

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

第一步小車運動過程中沒有進行狀態(tài)增廣:

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

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

三、實驗分析

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

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

圖1 移動機器人仿真環(huán)境

圖2 實驗路徑跟蹤圖

圖3 X軸誤差比較

圖4 Y軸誤差比較

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

四、結(jié)論

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