葉羽泠，蔡樂才，黃洪斌，肖體剛

（1.四川輕化工大學(xué)自動(dòng)化與信息工程學(xué)院，四川 自貢 643000；2.宜賓學(xué)院三江人工智能與機(jī)器人研究院，四川 宜賓 644000）

引言

隨著人工智能的快速發(fā)展，無(wú)人駕駛技術(shù)廣泛應(yīng)用于各類車輛、機(jī)械設(shè)備以及移動(dòng)機(jī)器人，實(shí)時(shí)定位與地圖構(gòu)建（Simultaneous Localization and Mapping algorithm，SLAM）［1-3］作為自動(dòng)駕駛的核心技術(shù)，逐漸成為機(jī)器人領(lǐng)域研究的重要組成部分。較為經(jīng)典的有擴(kuò)展卡爾曼濾波 SLAM算法（Extended Kalman filter-SLAM，EKFSLAM）［4］，該算法基于貝葉斯理論，建立了一種高斯函數(shù)框架，對(duì)SLAM的方法做出了基本定位。擴(kuò)展卡爾曼濾波算法在面對(duì)非線性較強(qiáng)的定位與建圖環(huán)境下，關(guān)鍵點(diǎn)尋找的計(jì)算難度會(huì)有所加大，因此定位效率顯著降低；Fast-SLAM［5］算法基于一種蒙特卡洛采樣定律，建立起一種數(shù)學(xué)模型。盡管相比于EKF-SLAM算法，F(xiàn)ast-SLAM算法能夠消除非線性干擾的影響，但隨著位姿計(jì)算迭代次數(shù)的增加，會(huì)出現(xiàn)誤差累加的情況。結(jié)合上述兩種算法的優(yōu)勢(shì)，Sun等人［6］提出了混合 SLAM算法（Hybrid-SLAM），該算法使用Fast-SLAM作為前端里程視覺器信息采集的處理方法，同時(shí)使用EKF-SLAM作為后端非線性優(yōu)化的位姿矩陣函數(shù)的處理。但由于Hybrid-SLAM直接采用機(jī)器人運(yùn)動(dòng)方程來(lái)判斷其位姿數(shù)據(jù)，一定程度上會(huì)影響位姿采樣結(jié)果的精確度［7］。

而在基于前端視覺里程與后端優(yōu)化的卡爾曼濾波算法的研究方面，閆德立等人［8］提出了一種新的變步長(zhǎng)LMS算法，將恒定容積卡爾曼濾波融合于SLAM，在容積點(diǎn)采樣方面使用李群變量實(shí)現(xiàn)了容積變換，并表示出各個(gè)時(shí)刻機(jī)器人的視覺位姿和協(xié)方差。但在容積變換傳遞狀態(tài)均值及方差的濾波方法方面，位姿精度提高的步驟較為繁瑣復(fù)雜；燕必希等人［9］在研究搭載單目相機(jī)的機(jī)器人運(yùn)動(dòng)目標(biāo)定位時(shí)，對(duì)加速度進(jìn)行自適應(yīng)處理，根據(jù)先前位置信息的關(guān)聯(lián)函數(shù)，使用卡爾曼濾波算法得到設(shè)備的加速度和速度，從而建立運(yùn)動(dòng)方程和預(yù)測(cè)目標(biāo)位置。但缺乏回環(huán)檢測(cè)步驟，導(dǎo)致誤差逐漸積累；穆紹碩等人［10］在研究提高機(jī)載光電平臺(tái)目標(biāo)狀態(tài)位姿精度調(diào)整問題時(shí)，在使用擴(kuò)展卡爾曼濾波的前提下，采用蒙特卡羅法目標(biāo)定位的方法對(duì)卡爾曼濾波算法進(jìn)行了改進(jìn)，并分析了測(cè)量誤差，提升了機(jī)器人自動(dòng)駕駛設(shè)備系統(tǒng)的穩(wěn)定性；趙偉等人［11］在使用非線性濾波方法來(lái)處理非線性系統(tǒng)的魯棒控制時(shí)，為了降低或者消除系統(tǒng)的隨機(jī)性，將多種類型的容積卡爾曼濾波進(jìn)行奇異值分解，在基于卡爾曼濾波算法的對(duì)比上，展開了使用容積法則的系統(tǒng)魯棒性研究。此外，無(wú)跡卡爾曼濾波（Unscented Kalman Filtering，UKF）作為一種較為經(jīng)典的卡爾曼濾波方法，Bosov等人［12］選定了不同的信標(biāo)點(diǎn)進(jìn)行權(quán)值采樣，并通過非線性函數(shù)的后端優(yōu)化使用了無(wú)跡變換（Unscented Transformation，UT）的方式，得到機(jī)器人設(shè)備因外界干擾產(chǎn)生隨機(jī)狀態(tài)變化的基本特點(diǎn)。

以上幾種基于前端視覺里程與后端優(yōu)化的卡爾曼濾波算法的研究探索以及其他改進(jìn)的SLAM算法，普遍存在關(guān)鍵點(diǎn)尋找困難、定位精度低、建圖時(shí)間長(zhǎng)等問題。本文提出一種基于位姿自適應(yīng)的卡爾曼濾波的SLAM算法，從卡爾曼濾波算法的基本模型得出位姿基本狀態(tài)，提出了一種關(guān)鍵信標(biāo)尋找的自適應(yīng)的路徑規(guī)劃方法。與前文提到的各類改進(jìn)SLAM算法相比，其定位精度和循跡的準(zhǔn)確性明顯提升，建圖的時(shí)間顯著降低。

1 SLAM循跡的基本問題

自動(dòng)駕駛的機(jī)器人設(shè)備同步定位與地圖創(chuàng)建的一般工作流程如下：首先視覺里程計(jì)通過外部傳感器，包括激光雷達(dá)、深度攝像頭等傳感器采集相鄰的兩幅圖像信息或障礙物特點(diǎn)，估算機(jī)器人設(shè)備的運(yùn)動(dòng)姿態(tài)變化［13-14］。其次使用閉環(huán)檢測(cè)的算法，以修正位姿狀態(tài)因時(shí)間產(chǎn)生的累加誤差，判斷機(jī)器人設(shè)備運(yùn)動(dòng)后是否重新回到先前出現(xiàn)的位置［15-16］。由于經(jīng)過前端視覺里程計(jì)和回環(huán)檢測(cè)并計(jì)算出的數(shù)據(jù)結(jié)果存在累加誤差，因此最后需要進(jìn)行后端非線性優(yōu)化，得到更加精確的位姿狀態(tài)計(jì)算結(jié)果［17］。最后將得到的位姿數(shù)據(jù)使用關(guān)聯(lián)矩陣的形式表現(xiàn)出，同時(shí)依靠相關(guān)數(shù)學(xué)模型建立起與任務(wù)要求對(duì)應(yīng)的地圖，最終判斷出設(shè)備的具體位置以及前進(jìn)方向。基本原理如圖1所示。

圖1 基于SLAM的機(jī)器人信標(biāo)循跡

圖1中考慮將機(jī)器人運(yùn)動(dòng)場(chǎng)地看作一個(gè)二維平面，在這個(gè)平面上分布著多個(gè)人為設(shè)定或者客觀存在的可用于定位的信標(biāo)點(diǎn)。信標(biāo)點(diǎn)1表示機(jī)器人的初始位置或者已經(jīng)尋找到的位置，信標(biāo)點(diǎn)2表示當(dāng)前位置，信標(biāo)點(diǎn)1和信標(biāo)點(diǎn)2之間的連線表示機(jī)器人運(yùn)行軌跡?，F(xiàn)在的任務(wù)便是尋找到下一個(gè)信標(biāo)點(diǎn)3的坐標(biāo)（X，Y），后續(xù)的信標(biāo)點(diǎn)以此類推。而循跡過程，就是使用相應(yīng)的數(shù)學(xué)方法，通過卡爾曼濾波的方式實(shí)現(xiàn)信標(biāo)點(diǎn)定位。

假使機(jī)器人的位姿描述為Xp，信標(biāo)點(diǎn)的位置為，則：

其中：uk表示控制輸入，μk表示高斯白噪聲分布，并且定義過程噪聲協(xié)方差矩陣為Qk。機(jī)器人的運(yùn)動(dòng)狀態(tài)觀測(cè)Zj可表示為：

其中：i表示在信標(biāo)點(diǎn)的序號(hào)，φk表示噪聲分布總和，初始協(xié)方差矩陣分布為Rk。信標(biāo)點(diǎn)定位的SLAM過程中，考慮uk和Zi（k）的值，求解Xp和的過程就是SLAM循跡的控制目標(biāo)。

2 位姿自適應(yīng)卡爾曼濾波算法

僅僅依靠信標(biāo)循跡的基本數(shù)學(xué)方法，由于缺乏位姿調(diào)整的部分，可能會(huì)產(chǎn)生噪聲干擾［18-19］。本文提出的位姿自適應(yīng)卡爾曼濾波的方法能夠在一定程度上解決此類問題。

2.1 位姿自適應(yīng)的描述模型

如果需要得到隨機(jī)信標(biāo)的位置并進(jìn)行估計(jì)，首先需要建立機(jī)器人的運(yùn)動(dòng)描述與觀測(cè)描述。位姿點(diǎn)是相對(duì)分散的信標(biāo)點(diǎn)，因此還需要離散化處理。離散化位姿描述模型為：

其中：F為機(jī)器人運(yùn)動(dòng)狀態(tài)轉(zhuǎn)移矩陣。然后建立觀測(cè)模型為：

其中：G為觀測(cè)矩陣。在基于卡爾曼濾波的位姿估計(jì)中，將位姿描述記作Xp=［Xpx，Xpy，Vx，Vy。定義在k時(shí)刻，（Xpx，Xpy）為機(jī)器人設(shè)備的位置坐標(biāo)；（Vx，Vy）機(jī)器人設(shè)備的速度；（）為機(jī)器人設(shè)備在被觀測(cè)到的第i個(gè)信標(biāo)位置。因此綜合位姿描述如式（5）～式（7）所示：

其中：E（k）表示機(jī)器人的運(yùn)動(dòng)狀態(tài)轉(zhuǎn)移矩陣。當(dāng)狀態(tài)更新時(shí)，下一時(shí)刻對(duì)狀態(tài)的預(yù)測(cè)可看作本時(shí)刻系統(tǒng)的狀態(tài)。一般來(lái)說(shuō)循跡過程至少包含2個(gè)信標(biāo)的位置信息。位姿自適應(yīng)的要求是：根據(jù)k時(shí)刻的機(jī)器人的位姿描述Xp（k）和控制輸入u（k），預(yù)測(cè)出系統(tǒng)在k+1時(shí)刻的位姿狀態(tài)Xp（k+1）、第i個(gè)信標(biāo)點(diǎn)的位置Zi（k+1以及機(jī)器人預(yù)估狀態(tài)的協(xié)方差矩陣P（k+1）。

2.2 SLAM的卡爾曼濾波方法的基本原理

卡爾曼濾波本質(zhì)上是一種實(shí)現(xiàn)最優(yōu)估計(jì)的算法。一般情況下利用系統(tǒng)的狀態(tài)描述公式，結(jié)合各類輸入輸出數(shù)據(jù)，對(duì)系統(tǒng)整體狀態(tài)進(jìn)行最優(yōu)估計(jì)［20］。但由于系統(tǒng)存在的噪聲干擾影響，所以卡爾曼濾波也可看作是一種噪聲消除方法。

當(dāng)信標(biāo)點(diǎn)的個(gè)數(shù)逐漸增加，預(yù)估狀態(tài)的維度數(shù)目也隨之增加。此時(shí)，位姿描述Xp可表述為Xp=［Xpx，Xpy，Vx，Vy，…］T，其中：（）表示新增的坐標(biāo)。

可觀測(cè)的向量表示為：

其中：G表示觀測(cè)矩陣。觀測(cè)的目的是根據(jù)第k+1時(shí)刻對(duì)第i個(gè)信標(biāo)的觀測(cè)Zi（k+1）值，構(gòu)造新的位姿觀測(cè)點(diǎn)Ti（k+1）與新的協(xié)方差矩陣Si（k+1）。

2.3 卡爾曼濾波位姿自適應(yīng)方式

當(dāng)建立起機(jī)器人設(shè)備的協(xié)方差和觀測(cè)矩陣之后，就可以建立基于卡爾曼濾波的位姿自適應(yīng)方法，將去噪聲的狀態(tài)向量和關(guān)鍵信標(biāo)點(diǎn)相對(duì)比，從而確定觀測(cè)信標(biāo)點(diǎn)的方向和距離［21］。當(dāng)位姿狀態(tài)過于陳舊時(shí)，會(huì)造成機(jī)器人定位軌跡滯后。位姿自適應(yīng)的調(diào)整方法可由式（11）～式（13）表示：

其中：M為介質(zhì)矩陣，Xp為位姿矩陣，P為初始協(xié)方差。系統(tǒng)位姿自適應(yīng)主要作用就是使機(jī)器人始終保持最新的位姿狀態(tài)，防止過于陳舊的位姿狀態(tài)用于信標(biāo)點(diǎn)定位。然后根據(jù)預(yù)測(cè)與觀測(cè)完成位姿更新后，通過自主消除噪聲的方式，完成信標(biāo)循跡的任務(wù)形成。這個(gè)過程可看作是自適應(yīng)性的調(diào)整過程。

同時(shí)，新的位姿點(diǎn)也存在噪聲的可能。為了討論噪聲帶來(lái)的影響，現(xiàn)在定義仿真環(huán)境下，信標(biāo)測(cè)量噪聲為ω、距離噪聲為ωd、速度噪聲為ωv、過程噪聲為ωp。這四個(gè)參數(shù)統(tǒng)稱為噪聲參數(shù)。在本文提出的位姿自適應(yīng)卡爾曼濾波的SLAM循跡算法中，定義初始協(xié)方差描述為Pk，測(cè)量噪聲協(xié)方差描述為Rk，過程噪聲協(xié)方差描述為Qk，分別表示為式（14）、式（15）和式（16）：

機(jī)器人系統(tǒng)的抗噪聲能力也是位姿自適應(yīng)能力的另一種表現(xiàn)方式。因此，不同噪聲參數(shù)的值會(huì)對(duì)不同的噪聲描述產(chǎn)生影響。而位姿自適應(yīng)的方式是通過自主調(diào)整噪聲參數(shù)的值來(lái)實(shí)現(xiàn)各種噪聲描述保持在最優(yōu)范圍內(nèi)，并對(duì)運(yùn)行軌跡造成影響。

3 位姿自適應(yīng)SLAM仿真實(shí)驗(yàn)

本文使用的虛擬機(jī)器人循跡定位，可以在一定程度上模仿真實(shí)機(jī)器人的運(yùn)動(dòng)軌跡，能夠基于SLAM算法的基本原理進(jìn)行定位建圖，實(shí)現(xiàn)實(shí)時(shí)定位與路徑規(guī)劃的控制目標(biāo)。實(shí)際機(jī)器人的運(yùn)動(dòng)狀態(tài)描述為：

其中：Lk表示時(shí)刻機(jī)器人k時(shí)刻的位姿狀態(tài)描述，αk-1表示機(jī)器人轉(zhuǎn)向的角度，m為機(jī)器人軸距，θk為速度矢量角。

而在信標(biāo)循跡的仿真研究中，借鑒實(shí)際機(jī)器人的運(yùn)動(dòng)狀態(tài)描述，仿真機(jī)器人自動(dòng)循跡車輛的觀測(cè)矩陣為：

其中：（d Xi，d Yi）表示第i個(gè)路標(biāo)的位置，d Xk、d Yk和 θk共同組成了機(jī)器人的觀測(cè)位姿描述，D為機(jī)器人與信標(biāo)點(diǎn)之間的距離，γ表示機(jī)器人前進(jìn)方向與路標(biāo)的夾角。

3.1 信標(biāo)點(diǎn)循跡的仿真研究

本文實(shí)驗(yàn)仿真研究在操作系統(tǒng)為Windows10，CPU為 Intel（R）core（TM）i7（2.6 GHz），MATLAB版本為2016a，內(nèi)存為8 GB的環(huán)境下進(jìn)行仿真實(shí)驗(yàn)。分別對(duì)Hybrid-SLAM（圖2）、Fast-SLAM（圖3）、EK-SLAM（圖4）和本文算法（圖5）使用MATLAB進(jìn)行分析，并比較各個(gè)算法之間的信標(biāo)定位和路徑規(guī)劃精度差異。由于實(shí)驗(yàn)條件在仿真環(huán)境下進(jìn)行，圖（2）～圖（5）中的坐標(biāo)值均為定義為單位長(zhǎng)度，其中各圖的（a）中紅色點(diǎn)為信標(biāo)點(diǎn)，（b）中的藍(lán)色的軌跡為模擬機(jī)器人運(yùn)動(dòng)軌跡，（c）中圓圈圖為信標(biāo)點(diǎn)尋找過程。

圖2 Hybrid-SLAM算法得到的循跡仿真圖

圖3 Fast-SLAM算法得到的循跡仿真圖

圖4 EK-SLAM算法得到的循跡仿真圖

圖5 位姿自適應(yīng)卡爾曼濾波SLAM算法得到的循跡仿真圖

對(duì)比仿真實(shí)驗(yàn)結(jié)果可以明顯發(fā)現(xiàn)，相對(duì)于其他三種算法，由于采用了基于卡爾曼濾波的位姿自適應(yīng)模型，本文算法模擬的機(jī)器人運(yùn)動(dòng)，經(jīng)過20個(gè)采樣信標(biāo)點(diǎn)時(shí)，定位點(diǎn)的精確度較高，基本上能與設(shè)定的虛擬信標(biāo)點(diǎn)重合，能夠?qū)崿F(xiàn)按照預(yù)計(jì)的路徑行駛。

3.2 定位與循跡誤差比較

根據(jù)2.3的位姿自適應(yīng)原理，信標(biāo)點(diǎn)噪聲ω、距離噪聲ωd、速度噪聲ωv以及過程噪聲ωp都將直接影響初始協(xié)方差矩陣Pk、測(cè)量噪聲矩陣Rk和過程噪聲矩陣Qk，從而對(duì)SLAM算法的定位精度造成影響。而距離測(cè)量噪聲ωd和速度測(cè)量噪聲ωv對(duì)初始協(xié)方差矩陣Pk和噪聲矩陣Rk的影響尤為嚴(yán)重。因此本文在幾種不同的ωd和ωv條件下，對(duì)20個(gè)信標(biāo)點(diǎn)處的橫縱方向偏差以及速度矢量夾角進(jìn)行仿真分析。每個(gè)定位信標(biāo)點(diǎn)處，模擬機(jī)器人的實(shí)際位姿與設(shè)定信標(biāo)的X方向和Y方向的坐標(biāo)偏差，以及速度矢量和行進(jìn)軌跡的夾角θ的偏差，如圖6～圖9所示。

圖6 位置角度誤差對(duì)比（保持ωd=1.5不變，ωv=0.5）

圖7 位置角度誤差對(duì)比（保持ωd=1.5不變，ωv=1.0）

圖8 位置角度誤差對(duì)比（保持ωd=3.0不變，ωv=0.5）

圖9 位置角度誤差對(duì)比（保持ωd=3.0不變，ωv=1.0）

由圖6～圖9可以看出，本文算法所模擬的機(jī)器人信標(biāo)循跡定位點(diǎn)與實(shí)際信標(biāo)點(diǎn)的偏差，無(wú)論是水平X方向的誤差，還是豎直Y方向的誤差，以及速度矢量角度的偏差，總體來(lái)說(shuō)均小于Hybrid-SLAM、Fast-SLAM、EKSLAM等幾種改進(jìn)的SLAM算法的誤差。由于本文算法的總體角度偏差較低，因此在整個(gè)循跡行駛過程中，在路徑規(guī)劃方面具備一定的魯棒性，故循跡效果更好。比較平均誤差（表1）可以發(fā)現(xiàn)，在每個(gè)信標(biāo)的樣點(diǎn)，使用Hybrid-SLAM、Fast-SLAM和 EK-SLAM的總誤差，均在0.5～0.6個(gè)單位長(zhǎng)度之間。而本文使用的位姿自適應(yīng)卡爾曼濾波SLAM算法，機(jī)器人路徑規(guī)劃的實(shí)際位置與信標(biāo)的坐標(biāo)的平均總偏差只有0.2個(gè)單位長(zhǎng)度左右，可見定位的精確度明顯優(yōu)于另外三種算法。對(duì)比每個(gè)信標(biāo)點(diǎn)處的速度矢量和運(yùn)動(dòng)軌跡的夾角值平均偏差（表2），可以看出相對(duì)于其他三種算法，本文算法的角度偏差較小，意味著循跡精度更加精確。

表1 四種算法的平均誤差比較

表2 四種算法的平均角度誤差對(duì)比

3.3 幾種不同算法的時(shí)間對(duì)比

循跡時(shí)間即完成整個(gè)信標(biāo)循跡過程的耗時(shí)。通過對(duì)比四種算法的循跡時(shí)間（表3）可以知道，本文算法的循跡總耗時(shí)較低，即完成整個(gè)信標(biāo)循跡過程的速度較其他三種算法較快。因此，本文算法在機(jī)器人整體信標(biāo)循跡速度方面同樣具備優(yōu)勢(shì)。

表3 四種算法的循跡時(shí)間比較

4 結(jié)束語(yǔ)

機(jī)器人實(shí)時(shí)定位與地圖構(gòu)建領(lǐng)域的發(fā)展趨勢(shì)，要求定位速度再快、規(guī)劃的路徑更準(zhǔn)確，并對(duì)關(guān)鍵點(diǎn)的尋找精度有著更高要求。而信標(biāo)尋找作為機(jī)器人定位與路徑規(guī)劃的一個(gè)熱點(diǎn)研究方向，目前仍存在較大的探索空間。仿真實(shí)驗(yàn)表明，本文提出的位姿自適應(yīng)卡爾曼濾波算法在SLAM信標(biāo)循跡的過程中，尋找關(guān)鍵信標(biāo)點(diǎn)的速度和路徑規(guī)劃的時(shí)間也相對(duì)較短，同時(shí)循跡精度更高，為解決機(jī)器人設(shè)備關(guān)鍵點(diǎn)信標(biāo)定位、路徑規(guī)劃和自動(dòng)駕駛等方面的問題，提供了一個(gè)新的思路。