基于Cholesky分解的改進(jìn)自適應(yīng)EKF-SLAM算法

程 璐，黃宜慶

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

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

1 算法研究

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

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

(1)初始化。

(1)

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

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

①時(shí)間更新：

(2)

(3)

(4)

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

②狀態(tài)更新：

(5)

(6)

(7)

(8)

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

③噪聲估計(jì)更新：

(9)

(10)

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

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

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

(11)

式中，矩陣L的計(jì)算公式如下：

(12)

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

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

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

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

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

(13)

(14)

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

(15)

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

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

(16)

(17)

(18)

(19)

(20)

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

2 仿真實(shí)驗(yàn)

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

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

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

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

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

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

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

3 總結(jié)

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