劉韜

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

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

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

引言

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

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

1.1 狀態(tài)向量

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

（1）

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

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

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

（2）

1.2 觀測(cè)模型

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

（3）

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

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

（4）

（5）

其中

2 EKF-SLAM過(guò)程

2.1 預(yù)測(cè)

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

（6）

（7）

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

（8）

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

2.2 更新

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

（9）

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

（10）

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

（11）

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

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

（12）

（13）

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

（14）

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

（15）

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

（16）

（17）

2.3 地圖構(gòu)建

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

（18）

（19）

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

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

（20）

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

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

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

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

4 結(jié)論

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

參考文獻(xiàn)

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

[2] 王耀南， 余洪山. 未知環(huán)境下移動(dòng)機(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.