EKF—SLAM算法在小行星轉(zhuǎn)速測定中的初步應(yīng)用與仿真實現(xiàn)
2015-05-30 13:19:46劉韜
科技資訊 2015年26期
劉韜
摘 要：基于擴展卡爾曼濾波的同步定位與地圖構(gòu)建算法（EKF-SLAM）是機器人自主定位領(lǐng)域的常用算法之一。本文將此算法應(yīng)用于小行星轉(zhuǎn)速測定中，針對具體問題建立系統(tǒng)模型，并在MATLAB中實現(xiàn)仿真，測試其可行性，為小行星探測工程中探測器的自主導航提供一種新的思路。
關(guān)鍵詞：小行星探測，自主導航，擴展卡爾曼濾波，同步定位與地圖構(gòu)建，轉(zhuǎn)速測定
中圖分類號： TP24 文獻標識碼：A 文章編號：1672-3791（2015）9（b）-0000-00
引言
近年來，世界范圍內(nèi)掀起了一波小行星探測的熱潮，越來越多的國家加入了小行星探測的行列。小行星探測根據(jù)具體任務(wù)的不同，包括了繞飛，著陸，采樣返回等諸多方面。其中，小行星轉(zhuǎn)動速度的測定是至關(guān)重要的一部分。傳統(tǒng)的測定方法使用地面站數(shù)據(jù)，在長距離探測中會產(chǎn)生較大的延遲，實時性較低?，F(xiàn)代小行星探測中對于探測器的自主導航能力提出了更高的要求，因此，探索一種實時測定小行星轉(zhuǎn)速的方法變得十分必要。本文將機器人研究領(lǐng)域的EKF-SLAM算法應(yīng)用于小行星轉(zhuǎn)速測定中，并對此建立系統(tǒng)模型，在MATLAB中實現(xiàn)仿真，對此方法在實際應(yīng)用中的可行性給出了初步的判斷。
1 系統(tǒng)模型的建立
1.1 狀態(tài)向量
傳統(tǒng)的EKF-SLAM算法中狀態(tài)向量包含兩部分：機器人位姿與地圖特征信息。在本文所建立的系統(tǒng)模型中，二者分別對應(yīng)于探測器狀態(tài)與小行星表面特征信息。由于本研究主要涉及小行星轉(zhuǎn)速的測定，因此我們將小行星的狀態(tài)添加至狀態(tài)向量中，并在接下來的SLAM過程中對其進行估計。于是，系統(tǒng)的狀態(tài)向量可以表示為：
（1）
其中，為探測器狀態(tài)向量，為小行星狀態(tài)向量，為小行星表面特征狀態(tài)向量。
探測器狀態(tài)向量中包含其在全局坐標系中的位置和自身姿態(tài)；小行星狀態(tài)向量則包含其在全局坐標系中的位置，自身姿態(tài)和轉(zhuǎn)動速度；小行星表面特征狀態(tài)向量則包含小行星表面特征點在小行星局部坐標系中的三維坐標。
系統(tǒng)狀態(tài)向量對應(yīng)的協(xié)方差矩陣可表示為：
（2）
1.2 觀測模型
本文采用針孔攝像機觀測模型，觀測數(shù)據(jù)包含小行星表面特征點在圖像上的投影的二維坐標與小行星表面特征點距探測器的距離。
（3）
其中為小行星特征點在圖像上的二維坐標，為此特征點距探測器的距離，為觀測函數(shù)。
真實觀測值中加入了高斯白噪聲干擾：
（4）
（5）
其中
2 EKF-SLAM過程
2.1 預測
狀態(tài)預測函數(shù)將當前系統(tǒng)的狀態(tài)、動態(tài)模型輸入和系統(tǒng)噪聲作為參數(shù)，通過系統(tǒng)動態(tài)模型，計算系統(tǒng)狀態(tài)下一時刻的預測值及其協(xié)方差矩陣。
（6）
（7）
其中為系統(tǒng)狀態(tài)預測函數(shù)，為探測器位姿狀態(tài)輸入，為輸入噪聲。為系統(tǒng)狀態(tài)向量協(xié)方差矩陣，為探測器位姿狀態(tài)輸入噪聲協(xié)方差矩陣。為系統(tǒng)狀態(tài)預測雅可比矩陣，是擴展卡爾曼濾波方法中預測階段對非線性動態(tài)系統(tǒng)模型的線性化表示：
（8）
其中，，，分別表示探測器狀態(tài)向量、小行星狀態(tài)向量和小行星表面特征向量各自相對于上一時刻的變化，這三者的變化被認為是相互獨立的。
2.2 更新
系統(tǒng)狀態(tài)向量的更新階段利用測量值對預測值進行修正。對于所有的特征點，系統(tǒng)預測的測量值為：
（9）
對應(yīng)的誤差協(xié)方差矩陣可表示為：
（10）
為觀測函數(shù)的雅可比矩陣：
（11）
其中，，，分別表示觀測函數(shù)值相對于探測器狀態(tài)向量、小行星狀態(tài)向量和小行星表面特征向量預測值的變化。
測量余量及其對應(yīng)的協(xié)方差矩陣可表示為：
（12）
（13）
其中為測量誤差協(xié)方差矩陣：
（14）
最優(yōu)卡爾曼增益可以表示為：
（15）
最后利用最優(yōu)卡爾曼增益來修正系統(tǒng)的狀態(tài)：
（16）
（17）
2.3 地圖構(gòu)建
系統(tǒng)每次提取一個新的特征點時，需要將其加入系統(tǒng)狀態(tài)向量中以擴充地圖。
（18）
（19）
式中新特征點的真實觀測值，為觀測函數(shù)的反函數(shù)。
系統(tǒng)狀態(tài)的協(xié)方差矩陣變化為：
（20）
3 仿真結(jié)果及分析
本文采用Joan Sola編寫的EKF-SLAM工具包，在MATLAB中自主編寫模擬與測定模塊，在選用12個地圖表面特征點的仿真中得到如下結(jié)果。
圖 1. EKF-SLAM仿真結(jié)果
如圖1所示，整個仿真過程中，測量速度始終位于的置信區(qū)間內(nèi)，仿真結(jié)果收斂性較好，測量速度逐步趨近于真實速度。
4 結(jié)論
本文結(jié)合擴展卡爾曼濾波與機器人自主定位研究領(lǐng)域的同步定位與地圖構(gòu)建方法，為自主測定小行星轉(zhuǎn)速提供了一種新的思路。仿真結(jié)果表明，此方法應(yīng)用于小行星自主探測中具有一定的可行性，小行星轉(zhuǎn)速測定結(jié)果收斂，實時性較高，可以極大地提高小行星探測器的自主導航能力，但其計算復雜度在航天具體工程中還有待驗證。
參考文獻
[1] 劉宇飛. 深空自主導航方法研究及在接近小天體中的應(yīng)用 [D][D]. 哈爾濱工業(yè)大學， 2007
[2] 王耀南， 余洪山. 未知環(huán)境下移動機器人同步地圖創(chuàng)建與定位研究進展[J]. 控制理論與應(yīng)用， 2008， 25（1）： 57-65.
[3] V. Bissonnette and M. Dor. Characterization of a near-earth asteroid by simultaneous localization and mapping （slam） method. Technical report， Institut Superieur de l'Aeronautique et de l'Espace， June 2014.
[4] V. Bissonnette and M. Dor. Vision based autonomous characterization and navigation for asteroids. Technical report， Institut Superieur de l'Aeronautique et de l'Espace， March 2015.
[5] Sola J. Simulataneous localization and mapping with the extended Kalman filter[J]. unpublished. Available： http：//www. joansola. eu/JoanSola/eng/JoanSola. html， 2013.
