位姿自適應卡爾曼濾波SLAM信標循跡的研究

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

（1.四川輕化工大學自動化與信息工程學院，四川 自貢 643000；2.宜賓學院三江人工智能與機器人研究院，四川 宜賓 644000）

引言

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

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

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

1 SLAM循跡的基本問題

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

圖1 基于SLAM的機器人信標循跡

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

假使機器人的位姿描述為Xp，信標點的位置為，則：

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

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

2 位姿自適應卡爾曼濾波算法

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

2.1 位姿自適應的描述模型

如果需要得到隨機信標的位置并進行估計，首先需要建立機器人的運動描述與觀測描述。位姿點是相對分散的信標點，因此還需要離散化處理。離散化位姿描述模型為：

其中：F為機器人運動狀態(tài)轉移矩陣。然后建立觀測模型為：

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

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

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

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

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

可觀測的向量表示為：

其中：G表示觀測矩陣。觀測的目的是根據第k+1時刻對第i個信標的觀測Zi（k+1）值，構造新的位姿觀測點Ti（k+1）與新的協(xié)方差矩陣Si（k+1）。

2.3 卡爾曼濾波位姿自適應方式

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

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

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

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

3 位姿自適應SLAM仿真實驗

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

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

而在信標循跡的仿真研究中，借鑒實際機器人的運動狀態(tài)描述，仿真機器人自動循跡車輛的觀測矩陣為：

其中：（d Xi，d Yi）表示第i個路標的位置，d Xk、d Yk和 θk共同組成了機器人的觀測位姿描述，D為機器人與信標點之間的距離，γ表示機器人前進方向與路標的夾角。

3.1 信標點循跡的仿真研究

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

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

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

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

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

對比仿真實驗結果可以明顯發(fā)現(xiàn)，相對于其他三種算法，由于采用了基于卡爾曼濾波的位姿自適應模型，本文算法模擬的機器人運動，經過20個采樣信標點時，定位點的精確度較高，基本上能與設定的虛擬信標點重合，能夠實現(xiàn)按照預計的路徑行駛。

3.2 定位與循跡誤差比較

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

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

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

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

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

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

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

表2 四種算法的平均角度誤差對比

3.3 幾種不同算法的時間對比

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

表3 四種算法的循跡時間比較

4 結束語

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