一种长距离高可靠月面巡视自主导航方法

谭龙玉，贺 亮，彭 杨，王兆龙，曹 涛

（1.上海航天控制技术研究所，上海201109；2.上海市空间智能控制技术重点实验室，上海201109）

1 引言

月面巡视器是一种具有高度自主能力、适于在复杂非结构化月面环境工作的探测移动机器人［1］，为成功执行探测任务，必须具备高度的自主导航能力。当前月面巡视器自主导航方法主要有航位推算、惯性导航、天文导航和视觉导航等。惯性导航具有输出信息连续、短时间精度高的优点，但是其定位误差随时间积累［2］；基于视觉的导航方法更适用于短距离的利用标志点导航及障碍检测、路径规划等［3］；天文导航是一种传统自主导航方法，不需与外界进行任何信息交换，可同时提供位置和航向信息且精度不受时间、距离长短影响，但是实时性欠缺［4］。鉴于各种导航手段各有优缺点，将多种导航方式组合以取长补短提高系统综合性能的组合导航技术是目前主要的发展方向。Ardif JP［5］研究了利用双目视觉相机辅助惯性导航系统的高精度自主导航方法。针对月面巡视器导航的特殊要求，裴福俊［6］提出了天文导航与航位推算有机组合的自主导航系统。Ku⁃roda Y［7］提出一种利用对太阳高度的连续观测，通过最小二乘法求解天文位置圆非线性方程组，获得月面巡视器固定点位置的天文导航方法。美国国家航空航天局的“勇气号”和“机遇号”火星车综合利用了里程计、惯性导航系统、视觉里程计和太阳敏感器组合进行导航定位［8］。宁晓琳［9］针对惯导／视觉／天文组合导航相关文献中仅是分时段组合而没有充分利用三者信息的弊端，提出基于惯导／视觉／天文组合的行星车导航方法，但是其未考虑行星表面环境复杂、巡视过程中情况多变引起的无法准确确定系统量测噪声方差阵和多传感器异步量测问题。于永军［10］在研究惯性／星光／卫星组合导航算法时涉及到非同步测量问题，给出了异步集中滤波算法，但是没有考虑相关传感器量测时滞问题。

本文针对月面巡视导航系统量测噪声方差阵无法准确确定和多传感器异步量测问题，提出一种基于惯导／视觉／天文组合的长距离高可靠月面巡视自主导航方法，全时段充分利用惯导、视觉和天文导航各自的优势，通过采用异步量测特性的集中自适应滤波算法提高自主导航系统的导航精度和可靠性，并通过数学仿真和对比分析验证该方法的有效性。

2 基于惯导／视觉／天文组合的长距离高可靠自主导航

2.1 组合方案

由于状态量为小量时，一阶近似的线性方程就能够精确地描述状态量的传播规律，因此组合方案采用以导航参数误差为状态量的卡尔曼间接滤波方案。基于惯导／视觉／天文组合的月面巡视自主导航系统设计为以惯性导航误差方程进行递推，采用太阳敏感器敏感太阳的高度角和方位角作为天文观测系统的量测量，双目相机处理输出的姿态和平移信息作为视觉导航系统的量测量，利用卡尔曼滤波算法进行自主导航系统导航参数的最优估计，完成巡视器的自主导航。具体方案如图1所示。

图1 基于惯导／视觉／天文组合的自主导航方案Fig.1 Autonomous navigation scheme based on SINS／VNS／CNS

2.2 模型

2.2.1 系统状态方程

间接滤波采用月面巡视器捷联惯导系统导航参数误差为状态量，同时为了修正惯性仪表的测量误差，也将陀螺和加速度计的常值漂移作为状态量，即状态量 X ＝ ［δλ δL δvEδvNφEφNφUωxωyωzxy］T，依次表示为经度和纬度误差、东向和北向速度误差、东向北向和天向平台误差角、陀螺常值漂移和加速度计常值漂移。根据惯导系统误差方程建立系统状态方程并离散化如式（1）［11］：

式中，Xk和Xk－1分别为k时刻和k－1时刻的状态量， Φk，k－1为状态转移矩阵， Γk－1为噪声驱动阵，Wk－1为系统噪声。

2.2.2 系统量测方程

1）惯导／视觉测量方程

视觉测量时首先双目相机在两个导航位置分别成像，两个相机的影像间要有一定的重叠度；然后在相邻两站左图像重叠区域中选择共同的特征点，在同一站的立体图像间进行精确匹配；最后由匹配上的特征点采用光束法平差计算得到两站的相对位姿，如式（2）所示：

式中，crj( k)为k时刻匹配与跟踪成功的第j个特征点在相机坐标系下的位置坐标，crj( k －1)为k－1时刻匹配与跟踪成功的第j个特征点在相机坐标系下的位置坐标，为相机坐标系由k－1时刻到k时刻的平移矢量，为 k－1时刻到 k时刻的旋转矩阵。

根据式（2）可以得出巡视器本体系k时刻相对k－1时刻的旋转量和平移量如式（3）所示：

式中，Rbc和Tbbc分别为相机测量系到巡视器本体系的旋转矩阵和平移矢量，bRkk－1，CAM转化为四元数形式得到bqkk－1，CAM。 由惯导系统得出惯性相对运动参数如式（4）所示［12］：

式中，⊗表示四元数乘法，和分别为组合导航系统得到k－1时刻的地理系下位置矢量和姿态四元数，rk，SINS和分别为惯导捷联解算得到k时刻的位置矢量和姿态四元数。

以惯导和视觉导航系统相对旋转四元数之差和相对平移矢量之差作为观测量，建立改进的量测方程如式（5） ～ （7）所示［12］：

式中函数fMM( )定义为取矩阵M的3×3右下角矩阵，V1为测量白噪声。

2）惯导／天文测量方程

惯导／天文组合系统采用基于太阳敏感器观测角度原始信息的紧耦组合方案，其能够减小天文导航定位解算引入的误差，同时更利于系统量测噪声方差阵根据天文观测角度原始信息进行准确确定。

假设天文观测高度角和方位角分别为Hp和Ap，计算高度角和方位角分别为Hc和Ac，平台系和计算系下的单位矢量分别为pS和cS，则有式（8）：

展开得到式（9）：

式中，Cpn为地理系和平台系之间的方向余弦矩阵，Cnc为计算系和地理系之间的方向余弦矩阵。

将天文观测高度角、方位角和计算高度角、方位角的关系式Ap＝Ac＋ΔA，Hp＝Hc＋ΔH代入上式，ΔH和ΔA都是小量，取其一阶近似，得式（10）、（11）：

从而建立量测方程如式（12）：

式中，V2为测量白噪声，各元素如式（13）所示：

3）系统量测方程

系统量测方程如式（14）所示：

式中，H为系统的观测矩阵，V为系统的量测噪声。

式（2）和式（14）分别构成自主导航系统的状态方程和量测方程。

2.3 异步量测特性的集中自适应滤波算法

面向月面巡视的实际应用，基于惯性／视觉／天文组合导航滤波算法需要解决两类工程化问题，一是由于月面巡视器敏感器数量和种类较多、量测输出不同步引起的异步量测特性且输出有时滞问题；二是由于月面环境复杂、巡视过程中情况多变引起的无法准确确定系统量测噪声方差阵的问题。针对于此，采用异步量测特性的集中自适应滤波算法进行解决。

2.3.1 异步量测特性和输出时滞处理策略

假设捷联惯导的解算周期为TSINS，卡尔曼滤波离散周期为TD，天文系统的输出周期为TSUN，视觉系统的输出周期为TCAM，同时满足TD＝L×TSINS，TSUN＝ M × TD，TCAM＝ N × TD，ΔT ＝( M －N )× TD，L，M，N 为正整数。 敏感器异步测量时间关系图如图2所示。

图2 敏感器异步测量时间关系图Fig.2 Time diagram of asynchronous measurement

根据卡尔曼滤波的特性，系统的时间更新和量测更新可以独立进行，建立如下的处理策略：

1）在 tk＝ i×TD，m ＝ 1，2…M时刻，没有太阳敏感器和视觉系统的量测输出，利用系统状态转移矩阵只进行卡尔曼滤波器的时间更新；

2）在tk＝M×TD时刻，利用太阳敏感器的量测输出，完成卡尔曼滤波器的时间更新和量测更新；

3）在ΔT时间段内，进行卡尔曼滤波器的时间更新；

4）在tk＝N×TD时刻，利用视觉系统的量测输出，完成卡尔曼滤波器的时间更新和量测更新。

针对量测时滞问题，以天文导航系统量测为例进行介绍处理方法。首先利用数组记录下tk( k ＝ 1，2…M)时刻惯导系统状态信息，在tk＝M×TD时刻有量测输出时，利用该测量信息和数组中记录的tk时刻惯导状态信息进行卡尔曼滤波处理，获得tk时刻滤波修正参数和相应的均方误差阵；然后利用系统状态转移矩阵计算tk＋1时刻的修正参数和相应的均方误差阵，并对tk＋1时刻的惯导状态进行修正。

2.3.2 自适应滤波算法

为避免使用经典卡尔曼滤波方法时需要准确系统模型和噪声统计特性的弊端，采用自适应滤波算法，在滤波的同时，利用量测信息不断地在线估计和修正量测噪声统计特性以提高巡视器的导航精度和可靠性。

在卡尔曼滤波过程中，Rk表征外部量测的精度，Kk决定了对新息的利用程度。当外部量测噪声增大时，应该降低对新息的加权程度以避免污染导航系统，造成滤波器发散，即Kk应该减小。如下利用新息序列协方差实测值和理论值的比值作为调节因子，进行量测噪声方差阵的在线调节。新息序列dk＝Zk－Hk， 新息序列协方差矩阵理论值如式（15）所示：

式中，Zk为量测量，Hk为量测矩阵，Vk为量测噪声，为一步预测值，为状态估计和一步预测的残差，由tk－1时刻的量测获得。因为和 Vk无 直 接 关 系， 即 E []＝E[ Vk]＝ 0，上式可变换为式（16）：

新息序列协方差矩阵实测值如式（17）所示：

设W为滑动数据窗口长度，则新息序列协方差理论值和实测值可重新表达如式（18）、（19）：

定义γ为量测噪声调节因子，则γ＝将卡尔曼滤波方程中的增益方程Kk的计算调整

3 仿真分析

如式（20）所示，即可完成量测噪声方差阵的在线调节。

3.1 仿真参数设置

基于Matlab数学仿真软件进行月面巡视器的自主导航数学仿真与分析，根据目前常用的宇航级敏感器精度和导航计算机计算能力，仿真参数设置如表1所示。

表1 仿真参数Table 1 Simulation Parameters

3.2 结果与分析

3.2.1 CRLB 仿真

CRLB（Cramer⁃Rao Lower Bound）表示理想情况下滤波估计误差的性能极限，其可用于评价算法收敛性和滤波器的滤波性能。基于惯导／视觉／天文组合的月面巡视自主导航系统，估计误差＝ Xk的CRLB可写成式（21）：

式中，信息矩阵满足式（22）所示迭代关系［13］：

其中，Qk为系统噪声方差阵。

月面巡视器位置估计均方误差与CRLB仿真结果如图3所示。由仿真结果可以看出，基于惯导／视觉／天文组合的月面巡视自主导航系统均方误差与CRLB十分接近，表明了滤波器具有较优的性能。

图3 位置均方误差与CRLB曲线Fig.3 Mean square error and CLRB of position

3.2.2 不同组合模式对比仿真

月面巡视器的行进路径选用美国勘探者3号探测器的着陆点（月球 2°56′N，336°40′E）［14］作为起点进行运动。仿真时间设置为3600 s，惯导／视觉组合（SINS／CNS）、惯导／天文组合（SINS／VNS）以及惯导／视觉／天文组合（SINS／CNS／VNS）方式的位置和姿态误差曲线如图4和图5所示。

图4 不同组合模式平台误差角Fig.4 Errors of platform angle for different integra⁃tions

由仿真结果可以看出，基于惯导／视觉／天文组合的月面巡视自主导航系统具有较高的位置、速度和姿态估计精度，比惯导／视觉组合与惯导／天文组合有明显的提升。视觉导航系统的引入能够有效抑制捷联惯导系统位置和姿态的误差累积，从而为天文导航系统提供较高精度的平台基准。得益于平台基准和基于原始天文观测信息准确的量测建模，天文导航量测信息又对组合导航系统误差进行有效补偿，因而最终提高了巡视器自主导航系统的精度。

图5 不同组合模式位置误差Fig.5 Position errors for different integrations

3.2.3 巡视器状态变化仿真

月面巡视器以1 m／s的速度匀速行驶，同时航向角以 0.1°／s的角速度转动，在 100 ～150 s时，光学相机出现故障导致测量值误差增大，随后恢复正常状态；在300 s～350 s时，人为将光学相机测量精度提高，随后恢复正常状态。仿真时间设置为1800 s。

将异步测量特性的集中自适应滤波方法与普通集中滤波方法进行对比仿真，普通集中滤波方法采用量测噪声阵为常值的经典卡尔曼滤波方法，惯导／天文组合系统采用基于原始信息的紧耦组合方案，位置估计误差对比曲线如图6所示。

图6 位置误差对比Fig.6 Comparison of position error

由位置误差曲线结合数据统计结果可以看出，基于异步测量特性的集中自适应滤波方法误差均方根为26.68 m，滤波稳定后最大导航误差57.32 m。进一步地，在仿真过程中由于光学相机测量误差发生变化（包括增大和减小）导致普通集中卡尔曼滤波方法量测噪声与初始设置方差阵不匹配，因而估计误差较大；异步测量特性的集中自适应滤波方法得益于具有更精确特性的惯性／天文紧耦组合量测建模，且能够实时在线调整量测噪声方差阵，使其在两段光学相机测量波动期间均能较好地跟踪，滤波过程平稳、估计精度较高、鲁棒性较强。

4 结论

1）基于惯导／视觉／天文组合的月面巡视自主导航系统具有较高的位置、速度和姿态估计精度，能够满足月面巡视高精度自主导航的需求。

2）异步测量特性的集中自适应滤波方法通过实时在线调整噪声方差阵，使得导航系统滤波过程平稳，具有更高的估计精度较高和鲁棒性；同时，由于充分考虑了月面巡视器多敏感器量测信息的异步特性和量测时滞问题，其更接近于工程实际情况，工程可用性和可实现性也更好。

［1］ 娄路.基于视觉和MEMS⁃IMU融合的火星车导航定向技术［J］.航天控制， 2012， 30（4）： 32⁃37.

Lou L.An approach of attitude estimation by using vision⁃ai⁃ded IMU for Mars rover［J］.Aerospace Control， 2012， 30（4）： 32⁃37.（in Chinese）

［2］ 刘建业，曾庆化，赵伟，等.导航系统理论与应用［M］.西安：西北工业大学出版社，2010：334⁃335.

Liu JY，Zeng QH，Zhao W，et al.Navigation System Theo⁃ry and Application［M］.Xi’an： Northwestern Polytechnical University Press， 2010：334⁃335.（in Chinese）

［3］ Kuroda Y，Kurosawa T，Tsuchiya A，et al.Accurate localiza⁃tion in combination with planet observation and dead reckon⁃ing for lunar rover［J］.Proc.of 2004 IEEE International Confer⁃ence on Robotics and Automation， 2004， 16 （2）：2092⁃2097.

［4］ 全伟，刘百奇，宫晓琳，等.惯性／天文／卫星组合导航技术［M］.北京： 国防工业出版社，2011：32⁃33.

Quan W， Liu B Q， Gong X L， et al.INS／CNS／GNSS Inte⁃grated Navigation Technology ［M］.Beijing： National De⁃fense Industry Press， 2011：32⁃33.

［5］ Tardif JP，George M，Laverne M，et al.A new approach to vision⁃aided inertial navigation［C］ ／／The2010 IEEE／RSJIn⁃ternational Conference on Intelligent Robotsand Systems， Tai⁃pei， 2010：4161⁃4168.

［6］ 裴福俊，居鹤华，崔平远.基于联邦卡尔曼滤波的月球车自主导航方法［J］.宇航学报， 2009， 30（4）：145⁃150.

Pei F J， Ju H H， Cui P Y.Autonomous navigation method based on federated kalman filter for lunar rover［J］.Journal of Astronautics， 2009， 30（4）： 145⁃150.（in Chinese）

［7］ Kuroda Y，Kurosaw T.Accurate localization in combination with planet observation and dead Reckoning for lunar Rover［C］ ／／Proc.of 2004 IEEE International Conference on Ro⁃botics and Automation， New Orleans，2004：2092⁃2097.

［8］ 邸凯昌.勇气号和机遇号火星车定位方法评述［J］.航天器工程，2009，18（5）： 1⁃5.

Di K C.A review of spirit and opportunity rover localization methods［J］.Spacecraft Engineering， 2009， 18（5）： 1⁃5.（in Chinese）

［ 9 ］ Ning X L， Gui M Z， Xu Y Z， et al.INS／VNS／CNSintegrat⁃ed navigation method for planetary rovers［J］.Aerospace Sci⁃ence and Technology， 2016， 48（6）： 102⁃114.

［10］ 于永军，刘建业，熊智，等.非同步量测特性的惯性／星光／卫星组合算法研究［J］.仪器仪表学报，2011，32（12）： 2761⁃2767.

Yu Y J， Liu JY， Xiong Z， et al.Study on SINS／STAR／GPS integrated navigation algorithm with incoordinate interval char⁃acteristics［ J］.Chinese Journal of Scientific Instrument，2011， 32（12）： 2761⁃2767.（in Chinese）

［11］ 吴伟仁，王大轶.深空探测器自主导航原理与技术［M］.北京：中国宇航出版社，2011：212⁃213.

Wu W R，Wang D Y.Autonomous Navigation Principle and technology for Deep Space Probe［M］.Beijing： China Astro⁃nautic Publishing House， 2011： 212⁃213.（in Chinese）

［12］ 宁晓琳，徐勇志.一种改进的巡视器惯性／视觉组合导航模型［J］.中国空间科学技术，2015，2（1）： 9⁃18.

Ning X L， Xu Y Z.Improved INS／VNSintegrated navigation measurement［ J］.Chinese Space Science and Technology，2015， 2（1）： 9⁃18.（in Chinese）

［13］ Simandl M， Kralovec J， Tichavsky P.Filtering， predictive，and smoothing Cramer⁃Rao bounds for discrete⁃time nonlinear dynamic systems［J］.Automatica，2001， 37（3）：1703⁃1716.

［14］ 宁晓琳，蔡洪炜，吴伟仁，等.月球车的惯性／天文组合导航新方法［J］.系统工程与电子技术，2011，33（8）：1838⁃1844.

Ning X L， Cai H W， Wu W R， et al.INS／CNS integrated navigation method for lunar rover［J］.Systems Engineering and Electronics，2011， 33（8）： 1838⁃1844.（in Chinese）