基于陆标图像的天体定点着陆信息融合导航方法*

张晓文，李 骥，黄翔宇，王大轶

(1.北京控制工程研究所，北京100190;2.空间智能控制技术重点实验室，北京100190)

0 引言

未来的地外天体着陆探测任务出于具备更高安全性和获得更多高科学价值数据考虑，对探测器落点的精度提出了越来越高的要求，尤其是载人探测任务［1-2］.这就对星上的导航系统提出了新的挑战，因为精确制导的前提条件是探测器位置速度实时精确可知.

20世纪60年代末期美国阿波罗(Apollo)载人飞船登月时，星上导航系统主要由惯性测量单元和微波雷达构成［3］.2005年日本隼鸟号(Hayabusa)探测器附着Itokawa小行星时利用激光测距敏感器确定其高度，利用光学成像敏感器拍摄事先投放到小行星表面的人工陆标确定其相对小行星的水平速度［4-5］.2013年中国首个月球着陆探测器嫦娥三号成功登陆月球虹湾，其导航系统与阿波罗飞船相比，新增了激光测距敏感器和避障敏感器［6］.使用激光或微波作介质的测距敏感器和测速敏感器能够对高度和速度作出及时修正，与惯性导航相配合极大地提高了着陆过程的自主导航精度［7］.但是测距信息仅能对高度作出修正，不能提供相对天体表面的水平位置信息.利用光学成像敏感器拍摄的天体表面二维图像［8］，或是利用激光扫描雷达获取的天体地表三维图像［9］，则可以提供相对方位信息.此外，二维或三维地面图像除了用于导航还可用于对大坑或凸起等障碍的识别［10］，实现了敏感器的复用.

在获取了各个敏感器测量数据之后还需利用信息融合方法统一处理以给出着陆器位置速度最优估计.集中式滤波方法虽然能够对所有测量信息作最优融合处理，但是其滤波方程维数很高，不利于星上实时计算，同时该方法的容错性差，不易处理不同敏感器的时间同步性问题，因此不适合工程应用.基于协方差交叉理论的分布式滤波方法在众多信息融合方法中获得了越来越多的关注和认可.该方法与联邦滤波等分布式滤波方法相比主要优点是不要求局部状态估计相互独立或已知其相关性，同时其融合估计精度高于每个局部估计的精度［11-12］.

针对惯性导航误差不断累计，以及测距测速信息仅能修正高度和速度不能修正相对位置的缺点，将光学成像敏感器加入到导航系统中，通过拍摄天体表面已知特征点的二维灰度图像并与星上数字地形图匹配比较获取相对水平位置信息，从而显著提高了下降过程中相对水平位置估计的精度.针对集中式滤波以及联邦滤波的各种固有缺陷，设计了基于协方差交叉的分布式扩展卡尔曼滤波器，对多源测量信息作最优信息融合，提高了状态估计精度和算法鲁棒性.

1 导航系统方案

基于陆标图像的组合导航系统的组成见图1.图1中的左侧部分为敏感器，分别为惯性测量单元、测距敏感器、测速敏感器和光学成像敏感器.图1中右侧部分为星上软件算法模块，分别为姿态确定、图像处理、数字地形图和位置速度确定.惯性测量单元中的陀螺组件测量和输出3轴角度增量，经姿态确定算法处理得到姿态.惯性测量单元中的加速度计组件测量和输出3轴速度增量.惯性测量单元在初始对准之后不再依赖外部输入，因此惯性导航在着陆下降全程运行.测距敏感器向探测器下方发射波束测量该方向斜距.测速敏感器向探测器下方发射3束不共面的波束测量相对速度.光学成像敏感器拍摄前下方地面特征点的二维灰度图像.图像经图像处理算法计算给出陆标像素坐标.速度增量、斜距、相对速度和陆标像素等测量最终由位置速度确定算法统一处理给出探测器位置速度的实时精确估计.

图1 自主导航系统组成与信号流图Fig.1 Composition and signal flow of the autonomous navigation system

2 动力学方程

地外天体探测器的轨道动力学方程如下:

式中:r为探测器在惯性系中位置矢量，r为其模;v为探测器在惯性系中速度矢量;μ为中心天体引力常数;F为除引力外作用于探测器的合推力;m为探测器质量;aε为天体非球形引力和第三体引力等引起的摄动加速度.

3 测量方程

3.1 斜 距

测距敏感器输出的斜距的测量方程如下:

式中:sl为测距敏感器含误差的标度因数;Rm为天体参考半径;h(r)为天体数字高程函数;“·”为求数量积符号;Cib为本体系到惯性系的姿态转换阵;Al为测距波束在本体系指向;bl为测距敏感器常值偏差;nl为测距敏感器测量噪声，与距离相关.

3.2 相对速度

测速敏感器输出的相对速度的测量方程如下:

式中:su为测速敏感器含误差的标度因数;Au为测速波束在本体系指向;Cbi为惯性系到本体系的姿态转换阵;ωm为天体自转角速度矢量;“×”为求向量积符号;bu为测速敏感器常值偏差;nu为测速敏感器测量噪声.

3.3 陆标像素坐标

陆标像素坐标的测量方程如下:

式中:Kp为光学成像敏感器像素坐标转换阵;Cpb为本体系到光学成像敏感器测量系的姿态转换阵;ro为陆标在惯性系中位置矢量;p0为焦平面中心的像素坐标;np为光学成像敏感器测量噪声.

4 导航算法

导航算法中状态变量x取为探测器在惯性系中的位置矢量r和速度矢量v.由于式(1)所示的系统方程和式(2)及(4)所示的测量方程都是状态变量的非线性方程，因此子滤波器采用扩展卡尔曼滤波.各子滤波器给出的状态估计和误差协方差阵利用协方差交叉法进行信息融合.具体计算步骤如下.

(1)惯导外推

式中:下标k为迭代计算序号;为陀螺组合三轴等效常值漂移;Δt为导航周期.

式中Cib，k为由转换得到的姿态转换阵.

惯导外推为探测器提供了独立实时的外力作用下位置速度的基本估计.在没有引入斜距、相对速度和陆标像素坐标等测量信息之前导航系统仅执行惯导外推.

(2)误差协方差一步预测

式中:Φk，k-1为状态转移矩阵;Pk-1为误差协方差估计;Qk-1为系统噪声方差阵.

状态转移矩阵的计算公式为

式中I为单位阵.

(3)子滤波器测量更新

对当前导航计算周期内的每项有效测量构造一个基于扩展卡尔曼滤波的子滤波器，并用上标i来表示子滤波器的序号.记第i个有效测量的测量方程的右端表达式为hi(x)，对其求偏导得到线性化的k时刻的测量矩阵如下:

计算子滤波器的滤波增益矩阵和状态估计:

计算子滤波器的误差协方差阵

(4)基于协方差交叉的信息融合估计

计算子滤波器的误差协方差阵的迹

式中函数trace()表示求矩阵迹.

计算子滤波器的非负加权系数

式中n为子滤波器总数，即有效测量总数.

计算信息融合后的误差协方差阵和状态估计

下一轮导航计算重复上述步骤直到任务结束.

5 数学仿真

数学仿真采用文献［7］中描述的常推力显示制导下的着陆轨迹，下降初始高度为15 km，终端高度为100 m.导航系统在径向、法向和前向分别有1.2 km和1.2 m/s的初始位置速度误差.惯性测量单元中陀螺的随机漂移为0.2(°)/h(σ)，常值漂移为0.2(°)/h;加速度计的测量噪声为 0.002 m/s2(σ).测距敏感器的标度因数误差为0.10%(σ)，常值偏差为0.1 m，测量噪声为2%(σ).测速敏感器的标度因数误差为 0.13%(σ)，常值偏差为0.01 m/s，测量噪声为0.16 m/s(σ).陆标像素坐标的图像处理提取误差为0.1像素(σ).测距敏感器和测速敏感器的标度因数误差与常值偏差按照时间常数为100 s的一阶马尔科夫过程建模.除光学导航敏感器采样频率为0.1 Hz外，其余导航敏感器采样频率均为20 Hz.

系统噪声方差阵取为对角阵，前3个对角元素为0，后3个对角元素取1×10-6(m2/s4)，选择依据是惯性导航误差，以及数学仿真中实际滤波效果.记斜距估计为.斜距、相对速度、陆标像素坐标的测量噪声方差取值依次为0.1(像素)2，选择依据是敏感器误差源类型和大小，以及数学仿真中实际滤波效果.

数学仿真设定3种工况.工况1是导航系统仅进行惯性导航.工况2是导航系统不仅进行惯性导航，同时还在下降开始100 s后加入斜距和相对速度测量对高度和相对速度估计的修正.工况3是本文提出导航方法，即在工况2的基础上从110 s起加入陆标像素坐标测量对相对水平位置估计的修正.仿真结果见图2至图7所示的导航误差曲线.图2和图3是工况1的结果，其位置速度误差曲线随着时间推移而不断增大.图4和图5是工况2的结果.图4中天向位置误差在100 s后迅速下降，东向和北向位置误差在经过200 s的震荡后有所下降.图5中3个方向的速度估计误差在100 s后均迅速下降，北向速度误差有约60 s振荡过程.图4和图5结果表明斜距和相对速度测量引入后有效地修正了高度和速度.图6和图7是工况3的结果.图6中天向位置误差在100 s后迅速下降，东向和北向位置误差在110 s后迅速下降，表明引入陆标像素信息后位置估计立刻得到提高.图7中3个方向的速度估计误差在100 s后均迅速下降，北向速度误差有约20 s振荡过程.

图2 惯性导航位置误差Fig.2 Inertial navigation position estimate errors

图3 惯性导航速度误差Fig.3 Inertial navigation velocity estimate errors

图4 引入测距测速信息的导航位置误差Fig.4 Position estimate errors when slant rang and relative velocity measurements are uesd

图5 引入测距测速信息的导航速度误差Fig.5 Velocity estimate errors when slant rang and relative velocity measurements are used

将3种工况的终端导航误差列于表1中作比较.从表1可知工况1无论是位置误差还是速度误差均远大于另外2种工况.工况2的位置估计误差大于工况3结果达2个数量级，且主要是由水平位置估计误差引起，两者的高度估计误差相当.工况2和工况3的速度估计误差均小于0.1 m/s.该结果表明本文提出的自主导航方法获得导航结果在3个仿真工况中最优.

图6 引入陆标图像的导航位置误差Fig.6 Position estimate errors when landmark images are uesd

图7 引入陆标图像的导航速度误差Fig.7 Velocity estimate errors when landmark images are uesd

表1 终端导航误差Tab.1 Terminal navigation errors

6 结论

本文研究了一种基于陆标图像和信息融合的天体定点着陆自主导航方法.该方法首先在惯性导航加测距测速修正的基础上，将光学成像敏感器加入到导航系统，通过在下降过程中拍摄天体表面陆标图像以获得着陆器相对陆标的方位信息，显著提高水平位置确定精度.其次在进行状态估计时设计了基于协方差交叉的分布式扩展卡尔曼滤波器，对多源测量信息作最优融合，除了提高状态估计精度，还提高了算法鲁棒性，使得动态滤波过程适应多敏感器异步采样或是部分测量无效的情形.数学仿真结果验证了该方法的有效性，获得的位置速度估计在各个方向精度均很高.结果对比表明陆标图像的引入不仅起到了大幅减小水平位置估计误差的作用，同时还加快了滤波收敛速度.

［1］TODD A E，MARTIN H，JOSEPH E R.Altair navigation during trans-lunar cruise，lunar orbit，descent and landing［C］//AIAA Guidance，Navigation，and Control Conference.Washington D.C.:AIAA，2010:1-33.

［2］JOSEPH E R，ANDREW T V，ROBERT A W，et al.Optical navigation plan and strategy for the lunar lander Altair［C］//AIAA Guidance，Navigation，and Control Conference.Washington D.C.:AIAA，2010:1-40.

［3］EICHLER J.A performance study of the lunar module's landing radar system ［C］//AIAA Guidance，Control and Flight Dynamics Conference.Washington D.C.:AIAA，1967:1-24.

［4］GASKELL R，BARNOUIN-JHA O，SCHEERES D，et al.Landmark navigation studies and target characterization in the Hayabusa encounter with Itokawa［C］//AIAA/AAS Astrodynamics Specialist Conference and Exhibit.Washiongton D.C.:AIAA，2006:1-12.

［5］TAKASHI K，MASATOSHI M，MASASHI U，et al.Optical hybrid navigation and station keeping around Itokawa［C］//AIAA/AAS Astrodynamics Specialist Conference and Exhibit.Washiongton D.C.:AIAA，2006:1-15.

［6］张洪华，关轶峰，黄翔宇，等.嫦娥三号着陆器动力下降的制导导航与控制［J］.中国科学:技术科学，2014，44(4):377-384.ZHANG H H，GUAN Y F，HUANG X Y，et al.Guidance navigation and control for Chang'E-3 powered descent［J］.Scientia Sinica Technologica，2014，44(4):377-384.

［7］黄翔宇，张洪华，王大轶，等.月球软着陆的高精度自主导航与控制方法研究［J］.空间控制技术与应用，2012，38(2):5-9.HUANG X Y，ZHANG H H，WANG D Y，et al.Pinpoint autonomous navigation and control for lunar soft landing［J］.Aerospace Control and Application，2012，38(2):5-9.

［8］NEVEU D，BRUNEAN-DUBUC S，ALGER M，et al.Pinpoint lunar landing navigation using crater detection and matching:design and laboratory validation［C］//AIAA Guidance，Navigation，and Control Conference.Washington D.C.:AIAA，2012:1-25.

［9］崔平远，冯军华，朱圣英，等.基于三维地形匹配的月球软着陆导航方法研究［J］.宇航学报，2011，32(3):470-476.CUI P Y，FENG J H，ZHU S Y，et al.3D Terrain feature matching based navigation for lunar soft landing［J］.Journal of Astronautics，2011，32(3):470-476.

［10］TONISLAV I，ANDRES H，JOHN M C.Probabilistic hazard detection for autonomous safe landing［C］//AIAA Guidance，Navigation，and Control(GNC)Conference.Washington D.C.:AIAA，2013:1-13.

［11］LIGGINS M E，HALL D L，LLINAS J.Hand book of multisensor data fusion，theory and practice［M］.New York:CRC Press，2009:319-444.

［12］JULIER S J，UHLMANN J K.A non-divergent estimation algorithm in the presence of unknow correlation［C］//Proceedings of 1997 IEEE American Control Conference.New York:IEEE，1997:2369-2373.