EKF—SLAM算法在小行星轉(zhuǎn)速測定中的初步應(yīng)用與仿真實(shí)現(xiàn)

劉韜

摘 要：基于擴(kuò)展卡爾曼濾波的同步定位與地圖構(gòu)建算法（EKF-SLAM）是機(jī)器人自主定位領(lǐng)域的常用算法之一。本文將此算法應(yīng)用于小行星轉(zhuǎn)速測定中，針對具體問題建立系統(tǒng)模型，并在MATLAB中實(shí)現(xiàn)仿真，測試其可行性，為小行星探測工程中探測器的自主導(dǎo)航提供一種新的思路。

關(guān)鍵詞：小行星探測，自主導(dǎo)航，擴(kuò)展卡爾曼濾波，同步定位與地圖構(gòu)建，轉(zhuǎn)速測定

中圖分類號： TP24 文獻(xiàn)標(biāo)識碼：A 文章編號：1672-3791（2015）9（b）-0000-00

引言

近年來，世界范圍內(nèi)掀起了一波小行星探測的熱潮，越來越多的國家加入了小行星探測的行列。小行星探測根據(jù)具體任務(wù)的不同，包括了繞飛，著陸，采樣返回等諸多方面。其中，小行星轉(zhuǎn)動速度的測定是至關(guān)重要的一部分。傳統(tǒng)的測定方法使用地面站數(shù)據(jù)，在長距離探測中會產(chǎn)生較大的延遲，實(shí)時性較低?，F(xiàn)代小行星探測中對于探測器的自主導(dǎo)航能力提出了更高的要求，因此，探索一種實(shí)時測定小行星轉(zhuǎn)速的方法變得十分必要。本文將機(jī)器人研究領(lǐng)域的EKF-SLAM算法應(yīng)用于小行星轉(zhuǎn)速測定中，并對此建立系統(tǒng)模型，在MATLAB中實(shí)現(xiàn)仿真，對此方法在實(shí)際應(yīng)用中的可行性給出了初步的判斷。

1 系統(tǒng)模型的建立

1.1 狀態(tài)向量

傳統(tǒng)的EKF-SLAM算法中狀態(tài)向量包含兩部分：機(jī)器人位姿與地圖特征信息。在本文所建立的系統(tǒng)模型中，二者分別對應(yīng)于探測器狀態(tài)與小行星表面特征信息。由于本研究主要涉及小行星轉(zhuǎn)速的測定，因此我們將小行星的狀態(tài)添加至狀態(tài)向量中，并在接下來的SLAM過程中對其進(jìn)行估計(jì)。于是，系統(tǒng)的狀態(tài)向量可以表示為：

（1）

其中，為探測器狀態(tài)向量，為小行星狀態(tài)向量，為小行星表面特征狀態(tài)向量。

探測器狀態(tài)向量中包含其在全局坐標(biāo)系中的位置和自身姿態(tài)；小行星狀態(tài)向量則包含其在全局坐標(biāo)系中的位置，自身姿態(tài)和轉(zhuǎn)動速度；小行星表面特征狀態(tài)向量則包含小行星表面特征點(diǎn)在小行星局部坐標(biāo)系中的三維坐標(biāo)。

系統(tǒng)狀態(tài)向量對應(yīng)的協(xié)方差矩陣可表示為：

（2）

1.2 觀測模型

本文采用針孔攝像機(jī)觀測模型，觀測數(shù)據(jù)包含小行星表面特征點(diǎn)在圖像上的投影的二維坐標(biāo)與小行星表面特征點(diǎn)距探測器的距離。

（3）

其中為小行星特征點(diǎn)在圖像上的二維坐標(biāo)，為此特征點(diǎn)距探測器的距離，為觀測函數(shù)。

真實(shí)觀測值中加入了高斯白噪聲干擾：

（4）

（5）

其中

2 EKF-SLAM過程

2.1 預(yù)測

狀態(tài)預(yù)測函數(shù)將當(dāng)前系統(tǒng)的狀態(tài)、動態(tài)模型輸入和系統(tǒng)噪聲作為參數(shù)，通過系統(tǒng)動態(tài)模型，計(jì)算系統(tǒng)狀態(tài)下一時刻的預(yù)測值及其協(xié)方差矩陣。

（6）

（7）

其中為系統(tǒng)狀態(tài)預(yù)測函數(shù)，為探測器位姿狀態(tài)輸入，為輸入噪聲。為系統(tǒng)狀態(tài)向量協(xié)方差矩陣，為探測器位姿狀態(tài)輸入噪聲協(xié)方差矩陣。為系統(tǒng)狀態(tài)預(yù)測雅可比矩陣，是擴(kuò)展卡爾曼濾波方法中預(yù)測階段對非線性動態(tài)系統(tǒng)模型的線性化表示：

（8）

其中，，，分別表示探測器狀態(tài)向量、小行星狀態(tài)向量和小行星表面特征向量各自相對于上一時刻的變化，這三者的變化被認(rèn)為是相互獨(dú)立的。

2.2 更新

系統(tǒng)狀態(tài)向量的更新階段利用測量值對預(yù)測值進(jìn)行修正。對于所有的特征點(diǎn)，系統(tǒng)預(yù)測的測量值為：

（9）

對應(yīng)的誤差協(xié)方差矩陣可表示為：

（10）

為觀測函數(shù)的雅可比矩陣：

（11）

其中，，，分別表示觀測函數(shù)值相對于探測器狀態(tài)向量、小行星狀態(tài)向量和小行星表面特征向量預(yù)測值的變化。

測量余量及其對應(yīng)的協(xié)方差矩陣可表示為：

（12）

（13）

其中為測量誤差協(xié)方差矩陣：

（14）

最優(yōu)卡爾曼增益可以表示為：

（15）

最后利用最優(yōu)卡爾曼增益來修正系統(tǒng)的狀態(tài)：

（16）

（17）

2.3 地圖構(gòu)建

系統(tǒng)每次提取一個新的特征點(diǎn)時，需要將其加入系統(tǒng)狀態(tài)向量中以擴(kuò)充地圖。

（18）

（19）

式中新特征點(diǎn)的真實(shí)觀測值，為觀測函數(shù)的反函數(shù)。

系統(tǒng)狀態(tài)的協(xié)方差矩陣變化為：

（20）

3 仿真結(jié)果及分析

本文采用Joan Sola編寫的EKF-SLAM工具包，在MATLAB中自主編寫模擬與測定模塊，在選用12個地圖表面特征點(diǎn)的仿真中得到如下結(jié)果。

圖 1. EKF-SLAM仿真結(jié)果

如圖1所示，整個仿真過程中，測量速度始終位于的置信區(qū)間內(nèi)，仿真結(jié)果收斂性較好，測量速度逐步趨近于真實(shí)速度。

4 結(jié)論

本文結(jié)合擴(kuò)展卡爾曼濾波與機(jī)器人自主定位研究領(lǐng)域的同步定位與地圖構(gòu)建方法，為自主測定小行星轉(zhuǎn)速提供了一種新的思路。仿真結(jié)果表明，此方法應(yīng)用于小行星自主探測中具有一定的可行性，小行星轉(zhuǎn)速測定結(jié)果收斂，實(shí)時性較高，可以極大地提高小行星探測器的自主導(dǎo)航能力，但其計(jì)算復(fù)雜度在航天具體工程中還有待驗(yàn)證。

參考文獻(xiàn)

[1] 劉宇飛. 深空自主導(dǎo)航方法研究及在接近小天體中的應(yīng)用 [D][D]. 哈爾濱工業(yè)大學(xué)， 2007

[2] 王耀南， 余洪山. 未知環(huán)境下移動機(jī)器人同步地圖創(chuàng)建與定位研究進(jìn)展[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.