EKF—SLAM算法在小行星转速测定中的初步应用与仿真实现

刘韬

摘 要：基于扩展卡尔曼滤波的同步定位与地图构建算法（EKF-SLAM）是机器人自主定位领域的常用算法之一。本文将此算法应用于小行星转速测定中，针对具体问题建立系统模型，并在MATLAB中实现仿真，测试其可行性，为小行星探测工程中探测器的自主导航提供一种新的思路。

关键词：小行星探测，自主导航，扩展卡尔曼滤波，同步定位与地图构建，转速测定

中图分类号： TP24 文献标识码：A 文章编号：1672-3791（2015）9（b）-0000-00

引言

近年来，世界范围内掀起了一波小行星探测的热潮，越来越多的国家加入了小行星探测的行列。小行星探测根据具体任务的不同，包括了绕飞，着陆，采样返回等诸多方面。其中，小行星转动速度的测定是至关重要的一部分。传统的测定方法使用地面站数据，在长距离探测中会产生较大的延迟，实时性较低。现代小行星探测中对于探测器的自主导航能力提出了更高的要求，因此，探索一种实时测定小行星转速的方法变得十分必要。本文将机器人研究领域的EKF-SLAM算法应用于小行星转速测定中，并对此建立系统模型，在MATLAB中实现仿真，对此方法在实际应用中的可行性给出了初步的判断。

1 系统模型的建立

1.1 状态向量

传统的EKF-SLAM算法中状态向量包含两部分：机器人位姿与地图特征信息。在本文所建立的系统模型中，二者分别对应于探测器状态与小行星表面特征信息。由于本研究主要涉及小行星转速的测定，因此我们将小行星的状态添加至状态向量中，并在接下来的SLAM过程中对其进行估计。于是，系统的状态向量可以表示为：

（1）

其中，为探测器状态向量，为小行星状态向量，为小行星表面特征状态向量。

探测器状态向量中包含其在全局坐标系中的位置和自身姿态；小行星状态向量则包含其在全局坐标系中的位置，自身姿态和转动速度；小行星表面特征状态向量则包含小行星表面特征点在小行星局部坐标系中的三维坐标。

系统状态向量对应的协方差矩阵可表示为：

（2）

1.2 观测模型

本文采用针孔摄像机观测模型，观测数据包含小行星表面特征点在图像上的投影的二维坐标与小行星表面特征点距探测器的距离。

（3）

其中为小行星特征点在图像上的二维坐标，为此特征点距探测器的距离，为观测函数。

真实观测值中加入了高斯白噪声干扰：

（4）

（5）

其中

2 EKF-SLAM过程

2.1 预测

状态预测函数将当前系统的状态、动态模型输入和系统噪声作为参数，通过系统动态模型，计算系统状态下一时刻的预测值及其协方差矩阵。

（6）

（7）

其中为系统状态预测函数，为探测器位姿状态输入，为输入噪声。为系统状态向量协方差矩阵，为探测器位姿状态输入噪声协方差矩阵。为系统状态预测雅可比矩阵，是扩展卡尔曼滤波方法中预测阶段对非线性动态系统模型的线性化表示：

（8）

其中，，，分别表示探测器状态向量、小行星状态向量和小行星表面特征向量各自相对于上一时刻的变化，这三者的变化被认为是相互独立的。

2.2 更新

系统状态向量的更新阶段利用测量值对预测值进行修正。对于所有的特征点，系统预测的测量值为：

（9）

对应的误差协方差矩阵可表示为：

（10）

为观测函数的雅可比矩阵：

（11）

其中，，，分别表示观测函数值相对于探测器状态向量、小行星状态向量和小行星表面特征向量预测值的变化。

测量余量及其对应的协方差矩阵可表示为：

（12）

（13）

其中为测量误差协方差矩阵：

（14）

最优卡尔曼增益可以表示为：

（15）

最后利用最优卡尔曼增益来修正系统的状态：

（16）

（17）

2.3 地图构建

系统每次提取一个新的特征点时，需要将其加入系统状态向量中以扩充地图。

（18）

（19）

式中新特征点的真实观测值，为观测函数的反函数。

系统状态的协方差矩阵变化为：

（20）

3 仿真结果及分析

本文采用Joan Sola编写的EKF-SLAM工具包，在MATLAB中自主编写模拟与测定模块，在选用12个地图表面特征点的仿真中得到如下结果。

图 1. EKF-SLAM仿真结果

如图1所示，整个仿真过程中，测量速度始终位于的置信区间内，仿真结果收敛性较好，测量速度逐步趋近于真实速度。

4 结论

本文结合扩展卡尔曼滤波与机器人自主定位研究领域的同步定位与地图构建方法，为自主测定小行星转速提供了一种新的思路。仿真结果表明，此方法应用于小行星自主探测中具有一定的可行性，小行星转速测定结果收敛，实时性较高，可以极大地提高小行星探测器的自主导航能力，但其计算复杂度在航天具体工程中还有待验证。

参考文献

[1] 刘宇飞. 深空自主导航方法研究及在接近小天体中的应用 [D][D]. 哈尔滨工业大学， 2007

[2] 王耀南， 余洪山. 未知环境下移动机器人同步地图创建与定位研究进展[J]. 控制理论与应用， 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.