程 璐，黃宜慶

(安徽工程大學(xué) 電氣工程學(xué)院，安徽 蕪湖 241000)

即時定位與地圖構(gòu)建(SLAM)是近幾年用于定位導(dǎo)航與智能體地圖構(gòu)建的一種廣泛而有效的算法，它被用來解決在未知環(huán)境中進行創(chuàng)建和更新地圖以及定位計算未知環(huán)境下智能體位置的計算問題[1]。在進行地圖構(gòu)建和同時定位時，尋找最佳或次優(yōu)路徑規(guī)劃。在移動機器人路徑規(guī)劃和地圖構(gòu)建的領(lǐng)域，提出了很多優(yōu)化算法。其中路徑規(guī)劃分為全局路徑規(guī)劃和局部路徑規(guī)劃，全局路徑規(guī)劃方法主要是柵格法和A*算法[2]。其中，柵格法將工作空間分解成多個簡單區(qū)域，并且用0和1來表示障礙物信息，從而將復(fù)雜度物理環(huán)境轉(zhuǎn)變成為計算機算法能夠處理的抽象空間。并且在該抽象空間里通過搜索柵格圖找到一條從起始柵格點到目標柵格點的最優(yōu)路徑。其中，柵格的大小影響著最優(yōu)路徑規(guī)劃的速度和實時性，柵格較小，地圖中所儲存的環(huán)境信息比較多，規(guī)劃速度會降低，實時性會下降；柵格較大，地圖中所儲存的環(huán)境信息比較少，規(guī)劃速度加快，但是地圖信息的減少會導(dǎo)致地圖建立的模糊，不利于路徑規(guī)劃。因此，柵格的劃分和路徑規(guī)劃速度實時性需要進行取舍。文獻[3]采用柵格法建模，通過在遺傳算法中進行改進負反饋來設(shè)計全局路徑，并根據(jù)設(shè)計的全局路徑來將自由柵格和障礙柵格互相轉(zhuǎn)換從而進行局部避障。文獻[4]通過將柵格法和群智能算法結(jié)合來對焊接機器人進行路徑規(guī)劃，以防止其與工件碰撞。通過蟻群算法來對自由柵格進行最短路徑規(guī)劃，設(shè)計出兩個焊點之間的避障方法，然后通過分組競爭粒子群優(yōu)化算法優(yōu)化機器人的焊接時間，從而實現(xiàn)全局的路徑規(guī)劃。文獻[5]通過簡化路徑坐標點和計算拐點處的旋轉(zhuǎn)方向和旋轉(zhuǎn)角度來優(yōu)化A*算法，只保留路徑點中的起點、拐點和終點來減少坐標點，從而減少運算次數(shù)。通過計算出拐點處機器人的旋轉(zhuǎn)方向和旋轉(zhuǎn)角度來保證機器人能夠及時根據(jù)拐點信息來調(diào)整自身姿態(tài)。局部路徑規(guī)劃方法主要是人工勢場法[6]和動態(tài)窗口法DWA[7]。人工勢場法是將地圖環(huán)境抽象出人造受力場，其中目標點產(chǎn)生“引力”，而障礙物產(chǎn)生“斥力”。最后通過求合力來控制移動機器人的運動。人工勢場法主要用于局部環(huán)境中躲避動態(tài)障礙物，該方法結(jié)構(gòu)簡單，便于底層運算的實時控制，但其存在局部極值點，在狹窄環(huán)境中移動時，容易出現(xiàn)當(dāng)臨近目標點位置存在障礙物時不能發(fā)現(xiàn)路徑等問題[8-9]。而地圖構(gòu)建所使用的SLAM算法種類很多，EKF-SLAM算法結(jié)合了卡爾曼濾波和SLAM算法，用來提高路徑規(guī)劃和地圖構(gòu)建的準確性和快速性，文獻[10]中證明了該算法的一致性。將擴展卡爾曼濾波(EKF)的思想引入SLAM算法中形成的EKF_SLAM算法[11-12]可以很好地處理定位導(dǎo)航中的路徑規(guī)劃問題。文獻[13]中通過加入時變調(diào)節(jié)因子來優(yōu)化EKF-SLAM算法，提高了算法精度，但是相對運行時間有所增加。文獻[14]通過引入最優(yōu)平滑濾波來減少地圖更新的計算量，減少算法的復(fù)雜度，但是引入的噪聲會讓濾波器在接近邊緣處效果不佳。文獻[15]通過判斷量測數(shù)據(jù)中是否存在粗差來進行抗差迭代計算，提高了計算效率，但是粗差的引入可能會導(dǎo)致系統(tǒng)的收斂性降低。針對EKF-SLAM中存在的問題，使用基于改進Sage-Husa自適應(yīng)算法來優(yōu)化系統(tǒng)分析中的發(fā)散問題并提高系統(tǒng)的魯棒穩(wěn)定性。

1 算法研究

1.1 改進Sage-Husa自適應(yīng)算法

改進Sage-Husa自適應(yīng)算法是由Sage和Husa提出的自適應(yīng)濾波算法[16]，有較好的應(yīng)用成果[17-18]。通過在傳統(tǒng)EKF-SLAM中加入遺忘因子，并且證明收斂性和一致性[19]，來進行對傳統(tǒng)EKF-SLAM的優(yōu)化和改進，具體過程如下：

(1)初始化。

(1)

式中，X(0)是狀態(tài)方程的初始狀態(tài)；P(0)是協(xié)方差估計的初始狀態(tài)。

(2)當(dāng)k=1,2…時，開始迭代。

①時間更新：

(2)

(3)

(4)

式中,Pk是先驗協(xié)方差估計矩陣；Xk是先驗估計；Q是加入的誤差矩陣；ε是用來計算卡爾曼增益的殘差值。

②狀態(tài)更新：

(5)

(6)

(7)

(8)

式中，R為觀測噪聲的協(xié)方差；K為卡爾曼增益。

③噪聲估計更新：

(9)

(10)

從上式可以看出，Sage-Husa自適應(yīng)算法通過增加遺忘因子b來實時修正過程激勵噪聲和觀測噪聲，從而提高算法的收斂性。

1.2 基于Cholesky分解改進Sage-Husa自適應(yīng)EKF-SLAM算法

EKF-SLAM算法過程是將智能體傳感器獲得的數(shù)據(jù)使用擴展卡爾曼濾波進行預(yù)測更新，從而獲得智能體的位姿的聯(lián)合概率密度。在線性代數(shù)中，Hermitian正定矩陣通過Cholesky分解形成一個下三角矩陣及其共軛轉(zhuǎn)置矩陣的乘積。Hermitian正定矩陣A的Cholesky分解形式為，A=LLT，其中L是具有實對角線和正對角線的下三角矩陣。Cholesky分解和LU分解[20]相比，具有更高的穩(wěn)定性和更好的精確性。

(11)

式中，矩陣L的計算公式如下：

(12)

式中，要求i>j。通過式(12)就可以計算出Hermitian正定矩陣A的Cholesky分解矩陣L。

擴展卡爾曼濾波通過對非線性系統(tǒng)的一階線性化，很好地解決了非線性模型下時間更新和狀態(tài)更新的收斂性和穩(wěn)定性。但是由于在擴展卡爾曼濾波中的線性化是局部線性化，會導(dǎo)致連續(xù)隨機變量的密度不再是正態(tài)分布。初始狀態(tài)估計和預(yù)測模型的誤差都會導(dǎo)致擴展卡爾曼濾波發(fā)散，并且擴展卡爾曼濾波的預(yù)測協(xié)方差矩陣值比真實協(xié)方差矩陣值低，需要增加穩(wěn)定噪聲來減小統(tǒng)計意義上不一致的風(fēng)險。因此提出基于Cholesky分解改進Sage-Husa自適應(yīng)EKF-SLAM算法，過程如下：

(1)初始化智能體位置、運動速度，轉(zhuǎn)向角等動態(tài)性能。增加觀測量的隨機噪聲為高斯白噪聲。規(guī)劃路標位置以及地標軌跡。

(2)地標提取和數(shù)據(jù)關(guān)聯(lián)。利用傳感器的數(shù)據(jù)來提取地標數(shù)據(jù)，并且通過數(shù)據(jù)關(guān)聯(lián)來分析該地標是否為之前觀測過的地標。如果已觀測過，則忽略這個地標；如果未觀測過，則將這個地標加入到狀態(tài)更新矩陣中。

(3)位姿預(yù)測。使用Sage-Husa自適應(yīng)濾波方式對位姿進行預(yù)測，其中，式(13)是預(yù)測狀態(tài)估計，式(14)是預(yù)測協(xié)方差估計，兩者為時間更新方程。

(13)

(14)

當(dāng)?shù)螖?shù)增加的時候，如果整個濾波過程是收斂的，那么誤差的后驗協(xié)方差估計矩陣Pk會逐漸減小并趨向零，則可以將式(5)改為式(15)

(15)

此時暫時忽略不計測量噪聲的協(xié)方差估計矩陣的無偏性，即忽略不計Pk的影響，從而提高算法的運行速度并且其收斂性也不受影響。接著按照式(3)、式(4)進行預(yù)測。

(4)數(shù)據(jù)更新。假設(shè)P=SST，矩陣P經(jīng)過Cholesky分解，其中S是下三角矩陣。將擴展卡爾曼濾波中的誤差后驗協(xié)方差估計矩陣Pk用S的積分來代替，可以使濾波器的精度提高。則基于Cholesky分解改進Sage-Husa自適應(yīng)的更新方程如下：

(16)

(17)

(18)

(19)

(20)

(5)增加新特征到狀態(tài)矩陣中。將新的地標加到狀態(tài)矢量X和協(xié)方差矩陣P中，用于更新狀態(tài)預(yù)測。

2 仿真實驗

分別設(shè)置障礙物個數(shù)為65個、105個，路徑點個數(shù)為17個，移動智能體從原點(0,0)的位置沿著路徑點規(guī)劃好的線路進行運動，其中添加的噪聲是服從(0,1)分布式高斯白噪聲。分別采用EKF-SLAM算法，改進自適應(yīng)EKF-SLAM算法和基于Cholesky分解改進自適應(yīng)EKF-SLAM算法來進行實驗。在65個障礙物的情況下3個算法比較如表1所示。在65個障礙物的情況下3個算法路徑規(guī)劃的軌跡圖如圖1所示。在65個障礙物的情況下，分別使用3種不同算法進行路徑規(guī)劃和給定實際路徑規(guī)劃的最大誤差值隨時間的變化如圖2、圖3所示。在105個障礙物的情況下3個算法比如表2所示。在105個障礙物的情況下3個算法路徑規(guī)劃的軌跡圖如圖4所示。由于起始和終止的最大誤差跳躍幅度比較大，因此選取最大誤差相對平穩(wěn)的時間段進行繪圖。在105個障礙物的情況下，分別使用3種不同算法進行路徑規(guī)劃和給定實際路徑規(guī)劃的最大誤差值隨時間的變化如圖5、圖6所示。

通過表1、表2，圖1～圖6分析可得，隨著障礙物數(shù)量的增加，3個算法的運行時間都有所增加。改進自適應(yīng)EKF-SLAM算法相對于傳統(tǒng)EKF-SLAM算法，由于添加了遺忘因子，預(yù)測路徑和智能體實際運動路徑的最大誤差有所減小，并且其運行時間也會有所降低。而基于Cholesky分解的改進自適應(yīng)EKF-SLAM算法，通過Cholesky因式分解的方法提高了運行精度，并且通過改進自適應(yīng)算法優(yōu)化了運行速度，使得運行時間得以減少，其在X軸和Y軸的平均最大誤差也比傳統(tǒng)EKF-SLAM和改進自適應(yīng)SLAM要小，精確性和穩(wěn)定性更好。

表1 65個障礙物的情況下3個算法比較

表2 105個障礙物的情況下3個算法比較

圖1 65個障礙物的情況下3個算法的軌跡圖圖2 65個障礙物的情況下3個算法X軸最大誤差

圖3 65個障礙物的情況下3個算法Y軸最大誤差圖4 105個障礙物的情況下3個算法的軌跡圖

圖5 105個障礙物的情況下3個算法X軸最大誤差圖6 105個障礙物的情況下3個算法Y軸最大誤差

3 總結(jié)

對于傳統(tǒng)的EKF-SLAM算法所產(chǎn)生的運行時間較長和運行精度較低的問題，Sage和Husa提出了自適應(yīng)濾波算法，通過在傳統(tǒng)EKF-SLAM算法中增加遺忘因子來優(yōu)化算法，提高算法的計算速度和計算精度。在此基礎(chǔ)上，對自適應(yīng)EKF-SLAM中的誤差后驗協(xié)方差估計矩陣進行Cholesky分解，能夠更進一步提高算法的運行速度。通過MATLAB的仿真實驗得出，基于Cholesky分解的Sage-Husa自適應(yīng)算法的運行結(jié)果更接近于真實值，估計準確性更高，收斂速度也更快，運行時間更短。