本发明涉及一种行星着陆时光学导航最优路标选取方法，属于自主导航领域。
背景技术：
行星软着陆将是未来深空探测最复杂的任务之一，由于通信延迟及深空动力学环境的复杂性，采用传统的基于地面遥控的导航、控制模式已无法满足高精度软着陆的需要，这要求着陆器必须具有自主导航功能。行星表面存在大量的陨石坑、岩石等形貌特征，具有较高的可见性与可分辨性，各国学者对其展开了大量的研究，以这些地形对路标进行自主光学导航，能够获得较高的导航精度和性能。
以路标光学信息为主的行星软着陆自主导航方法已成为研究热点。其中如何在众多的导航路标中选取合适的路标以便达到期望的性能要求，是基于路标导航的一个关键技术，直接影响了软件算法的计算效率与探测器的自主定位能力，决定了探测任务是否能够成功完成，因此路标导航的自主选取方法是当前科技人员关注的重点问题之一。
在已发展的导航路标自主选取方法中，在先技术[1](参见Burschka D,Geiman J,Hager G.Optimal landmark configuration for vision-based control of mobile robots[C].2003IEEE International Conference on Robotics and Automation.2003,3:3917-22)，利用观测矩阵的条件数对平面内移动的机器人的导航选取方法进行了研究，分析了导航路标的位置对导航精度的影响，并给出了导航路标的选取方法。但是这种方法只适合应用在平面二维运动空间内的物体，并不适用于深空探测器这种空间三维运动的物体。
在先技术[2](参见黄翔宇,崔平远,崔祜涛.深空自主导航系统的可观性分析[J].宇航学报,2006,27(3):332-337)，利用误差协方阵的方式对导航路标的观测对导航精度的影响，该方法通过对误差协方差阵的奇异值分析，采用数值方法得到了导航路标位置对导航精度的影响程度。但是由于该方法采用的是数值计算方法，计算量大，因此这种方法只适合前期的任务分析及地面试验分析，而并不能应用于探测器在轨的自主导航路标选取。
在先技术[3](参见崔平远,朱圣英,徐瑞等.基于观测矩阵的深空探测器自主导航路标选取方法:中国,CN101782392A[P].2010-07-21)，利用路标视线信息和路标间的观测角构建观测矩阵，通过分析观测矩阵可观测度得到路标选取方法，比较所有的特征值之和，选出最优的三个路标，但是由于该方法需要计算视场内所有路标的特征值之和，当视场内路标数目较多时，计算量大，且当选取的最优路标数目大于三个时，该方法的计算将更加复杂，因此这种方法只适合视场内路标数目较少且只选取三个最优路标的情况，不适合在大量路标中选取多个最优路标的情况。
技术实现要素：
为解决目前基于路标导航的深空探测器在大量路标中自主选取多个导航路标时计算繁琐复杂的问题，本发明公开的一种行星着陆光学导航最优路标选取方法，要解决的技术问题是：在大量路标中较为快速实现行星着陆光学导航最优路标选取，确定深空探测器在目标天体固联坐标系下的位置及姿态，具有优化质量高、鲁棒性强、计算量小等优点。本发明适用于采用多路标的像素信息进行深空探测器位置、姿态自主确定的导航系统。
本发明目的是通过下述技术方案实现。
本发明公开的一种行星着陆光学导航最优路标选取方法，随机选取n(n≥3)个初始路标并认定其为当前最优路标，构建观测矩阵H并计算矩阵(HTH)-1的初始特征值之和λ0，随机选取新的路标计算特征值之和λ，通过对特征值之和λ进行迭代运算和比较，从某一较高的初始接受概率参数出发，在接受概率参数不断下降的同时结合概率突跳特性寻找行星着陆光学导航最优路标，即实现行星着陆光学导航最优路标选取。
本发明公开的一种行星着陆光学导航最优路标选取方法，包括步骤如下：
步骤1，随机选取n(n≥3)个初始路标并认定其为当前最优路标，计算目标天体坐标系下路标相对探测器的位置矢量
读取光学相机拍摄到的目标天体表面地形图像，从中随机选取n个导初始路标，得到路标在目标天体坐标系下的位置矢量及轨道预报给出的深空探测器相对目标天体的大致位置矢量计算目标天体坐标系下n个路标相对探测器的位置矢量
步骤2，利用步骤1得到的位置矢量计算观测矩阵的行向量构建观测矩阵H并计算矩阵(HTH)-1的特征值之和λ0。
令和分别为第i个与第j个路标的单位视线向量，其中：ri和rj为第i个与第j个路标到探测器的距离，对n个路标中第i个和第j个路标组合的观测矩阵的行向量进行计算
其中和为辅助向量，计算公式如下
其中，Aij为探测器到第i个和第j个路标观测视线所形成的观测角。
对于n个初始路标，任意选取两个路标，两两组合后得到的观测矩阵行向量共有n(n-1)/2个值。观测矩阵H由n个初始路标所对应的行向量构成，由观测矩阵H得到矩阵(HTH)-1，矩阵(HTH)-1始终是3×3矩阵。设矩阵(HTH)-1主对角线上的元素为akk(k＝1,2,3)，则n个初始路标所对应的矩阵(HTH)-1特征值之和λ0为
步骤3，预设最小特征值之和为λmin，令λmin＝λ0。
步骤4，给定迭代运算的初始参数。
给定初始接受概率参数T0，T0与特征值之和λ0相对应，接受概率参数T降低之前均迭代N次。设迭代的终止误差为ε，以迭代终止次数L为迭代的终止条件，即进行L次降低接受概率参数的迭代后特征值之和的平均变化小于终止误差ε，则终止迭代。
初始接受概率参数T0、每次接受概率参数T降低前的迭代次数N、迭代终止误差ε，迭代终止次数L即为给定迭代运算的初始参数。
步骤5，随机选取新的路标，计算新的特征值之和λ。
从初始路标的邻域中随机选取n个新的路标，得到路标在目标天体坐标系下的位置矢量及轨道预报给出的深空探测器相对目标天体的大致位置矢量计算目标天体坐标系下n个路标相对探测器的位置矢量
令和分别为第i个与第j个路标的单位视线向量，其中：ri和rj为第i个与第j个路标到探测器的距离，对n个路标中第i个和第j个路标组合的观测矩阵的行向量进行计算：
其中和为辅助向量，计算公式如下
其中，Aij为探测器到第i个和第j个路标观测视线所形成的观测角。
对于n个新路标，任意选取两个路标，两两组合后得到的观测矩阵行向量共有n(n-1)/2个值。观测矩阵H由n个新路标所对应的行向量构成，由观测矩阵H得到矩阵(HTH)-1，矩阵(HTH)-1始终是3×3矩阵。设矩阵(HTH)-1主对角线上的元素为akk(k＝1,2,3)，则n个新路标所对应的矩阵(HTH)-1特征值之和λ为
步骤6，计算特征值之和λ与λmin的差值dE，并判断差值dE是否小于0，根据判断结果执行步骤8或步骤7。
由步骤5得到新的矩阵(HTH)-1特征值之和λ，计算特征值之和λ与λmin的差值dE：
dE＝λ-λmin (9)
若差值dE小于0，执行步骤8，否则执行步骤7。
步骤7，计算接受概率p，并判断接受概率p是否大于[0,1]区间内的随机数，根据判断结果执行步骤8或步骤9。
根据梅特罗波利斯Metropolis接受准则，计算接受概率p
p＝exp(-(λ-λmin)/Tk) (10)
其中Tk为当前接受概率参数。
随机生成[0,1]区间内的数，若p大于该随机数，则执行步骤8，否则执行步骤9。
根据步骤7的判断，使步骤8选取的当前最优路标具有概率突跳特性。
步骤8，选取当前最优路标。
当步骤5随机选取新的路标满足步骤6判断差值小于0条件，则步骤5随机选取新的路标即为当前最优路标，当前最优路标对应的特征值之和λ为最小特征值之和，即令λmin＝λ。
当步骤5随机选取新的路标满足步骤7判断接受概率p大于[0,1]区间内随机数的条件，则步骤5随机选取新的路标即为当前最优路标，当前最优路标对应的特征值之和λ为最小特征值之和，即令λmin＝λ。
步骤9，判断迭代次数是否达到N。
判断结果为是，执行步骤10，判断结果为否，返回步骤5。
步骤10，判断是否满足步骤4所述的终止条件。
判断结果为是，执行步骤12，判断结果为否，执行步骤11。
步骤11，降低接受概率参数T。
依照接受概率参数T的更新函数降低接受概率参数T，接受概率参数T的更新函数为Tk+1＝αTk，其中k为当前迭代次数，Tk为当前接受概率参数，Tk+1为更新后的接受概率参数，α为常数，0<α<1。
更新接受概率参数Tk+1之后，返回步骤5。
步骤12，终止迭代，得到最小特征值之和λmin及其对应的最优路标。
迭代运算终止，得到最小特征值之和λmin，λmin所对应的路标即为选取的最优导航路标。
至此，完成行星着陆光学导航最优路标的选取。
还包括步骤13：在步骤12选取最优导航路标之后，利用轨道预报给出的着陆器当前位置先验估计值r*，确定测量角度的预报值以及线性向量hij，利用位置偏差量与测量夹角偏差量δAij之间的关系得到
δA＝Hδr (11)
其中δA是由第i个和第j个路标观测视线所形成的观测角的偏差δAij组成的矩阵，H是由第i个和第j个路标观测视线所形成的观测向量hij组成的矩阵。因而着陆器当前时刻的位置矢量为
r＝r*+(HTH)-1HTδA (12)
由于在探测器相机坐标系下，导航路标的位置表示为
将上式进行归一化，得
其中，利用导航路标的像元、像线坐标表示为
在已知着陆器位置的情况下，利用多矢量定姿原理，确定着陆器相对于目标天体固联系的姿态转移矩阵最优解为
其中
利用上述的位置解算公式(12)和姿态解算公式(16)，根据最优导航路标的观测信息能够解算探测器的位置和姿态，实现行星着陆光学导航。
有益效果：
1、本发明公开的一种行星着陆光学导航最优路标选取方法，随机选取n(n≥3)个初始路标并认定其为当前最优路标，构建观测矩阵H并计算矩阵(HTH)-1的初始特征值之和λ0，随机选取新的路标计算特征值之和λ，通过对特征值之和λ进行迭代运算和比较，从某一较高的初始接受概率参数出发，在接受概率参数不断下降的同时结合概率突跳特性寻找行星着陆光学导航最优路标，即结合概率突跳特性能够避免算法寻找到局部最优解，而无法寻找到全局最优解，实现本发明公开的一种行星着陆光学导航最优路标选取方法的优化质量高的优点。所述概率突跳特性指根据步骤7的判断，使步骤8选取的当前最优路标具有概率突跳特性。
2、本发明公开的一种行星着陆光学导航最优路标选取方法，对于n个初始路标，因此，任意选取两个路标，两两组合后得到的观测矩阵行向量共有n(n-1)/2个值。观测矩阵H由n个初始路标所对应的行向量构成，由观测矩阵H能够得到矩阵(HTH)-1，矩阵(HTH)-1始终是3×3矩阵，因此，矩阵(HTH)-1不会受到最优路标数目增加的影响，进而特征值之和λ不会受到最优路标数目增加的影响，即实现本发明公开的一种行星着陆光学导航最优路标选取方法的鲁棒性强的优点。
3、本发明公开的一种行星着陆光学导航最优路标选取方法，适用于采用多路标的像素信息进行深空探测器位置、姿态自主确定的导航系统，与穷举法相比，本发明方法不需要计算所有的待选路标，能够降低计算量，在大量路标中较为快速地寻找最优路标，确定深空探测器在目标天体固联坐标系下的位置及姿态。
附图说明
图1为本发明方法的流程图。
图2为本发明的具体实施方式中导航路标成像关系示意图。
图3为本发明方法选取最优路标的效果图，其中：(a)为选取三个最优路标的效果图，(b)为选取四个最优路标的效果图，(c)为选取五个最优路标的效果图，(d)为选取六个最优路标的效果图。
具体实施方式
为了更好地说明本发明的目的与优点，下面结合附图和实例对本发明做进一步说明。
令探测器与行星表面间的距离为7000m，导航相机的视场角为30°，导航相机拍摄到47个导航路标。
由于拍摄的导航路标像元、像线坐标中包含观测噪声，以及导航路标位置确定误差的存在，因此利用导航路标像素信息进行探测器位置、姿态确定必然存在误差。下面将通过对观测矩阵的分析，得到路标寻优算法的目标函数，进而给出最优导航路标的选取方法。
注意到对于最小二乘问题，有如下关系存在
X-X*＝(HTH)-1HT(Y-Y*) (1)
其中，X为待估量，X*为先验估计值，Y为观测值，Y*为预测观测值，H为观测矩阵，令则上式写成
可见矩阵(HTH)-1的特征值是系统可观测程度的一种度量方式，矩阵特征值越小，其特征向量所对应的状态偏差向量或状态偏差向量的线性组合所对应的误差越小，即求解精度越高。下面的路标寻优算法将采用矩阵(HTH)-1的特征值之和作为算法的目标函数，通过优化使矩阵(HTH)-1的特征值之和最小。
步骤1，随机选取n(n≥3)个初始路标并认定其为当前最优路标，计算目标天体坐标系下路标相对探测器的位置矢量
读取光学相机拍摄到的目标天体表面地形图像，从中随机选取n个导初始路标，得到路标在目标天体坐标系下的位置矢量及轨道预报给出的深空探测器相对目标天体的大致位置矢量计算目标天体坐标系下n个路标相对探测器的位置矢量
步骤2，利用步骤1得到的位置矢量计算观测矩阵的行向量构建观测矩阵H并计算矩阵(HTH)-1的特征值之和λ0。
探测器利用其携带的光学相机对导航路标成像，通过提取图像中导航路标的像元、像线坐标，获得导航路标在探测器坐标系下的指向方向，导航路标成像关系如图2所示。
令探测器相机坐标系相对目标天体坐标系的位置矢量和转换矩阵分别为和Cba，则在探测器相机坐标系下，导航路标的位置矢量为
其中，因目标天体坐标系为三维坐标系，转换矩阵Cba为三行三列矩阵。
第i个导航路标的像元pi、像线li坐标表示为
其中x,y,z为探测器在目标天体坐标系下的三轴位置坐标，xi,yi,zi为路标在目标天体坐标系下的三轴位置坐标，cba(a＝1,2,3；b＝1,2,3)为转换矩阵Cba中相应元素，f为光学相机的焦距。设选取的导航路标共有n个，则相应的观测量为
设第i个和第j个路标观测视线所形成的观测角为Aij，则
上式中为目标天体坐标系下第i个路标和第j个路标相对探测器的位置，ri,rj为第i个路标个第j个路标与探测器之间的距离。
该观测角能够利用光学图像中像素、像线坐标表示，即
上述几何描述也可用矢量公式表达，如利用和的内积关系，有
可见，上式为探测器位置与测量夹角Aij的关系式，而与探测器的姿态状态无关，因此，探测器的位置状态能够利用上式进行单独求解。
考虑到上式为非线性方程，直接求解比较困难，下面在小偏差线性化假设的条件下，对其线性化量测方程进行推导，得到位置偏差量与测量夹角偏差量δAij之间的近似线性关系
其中，观测矩阵行向量为
和为辅助向量，定义如下
和分别为第i个与第j个路标的单位视线向量
对于n个初始路标，任意选取两个路标，两两组合后得到的观测矩阵行向量共有n(n-1)/2个值。观测矩阵H由n个初始路标所对应的行向量构成，由观测矩阵H得到矩阵(HTH)-1，矩阵(HTH)-1始终是3×3矩阵。设矩阵(HTH)-1主对角线上的元素为akk(k＝1,2,3)，则n个初始路标所对应的矩阵(HTH)-1特征值之和λ0为
步骤3，预设最小特征值之和为λmin，令λmin＝λ0。
步骤4，给定迭代运算的初始参数。
给定初始接受概率参数T0，T0与特征值之和λ0相对应，接受概率参数降低之前均迭代N次。设迭代的终止误差为ε，以迭代终止次数L为迭代的终止条件，即进行L次降低接受概率参数的迭代后特征值之和的平均变化小于终止误差ε，则终止迭代。
初始接受概率参数T0＝100、每次接受概率参数T降低前的迭代次数N＝3000、迭代终止误差ε＝1e-6、迭代终止次数L＝500即为给定迭代运算的初始参数。
步骤5，随机选取新的路标，计算新的特征值之和λ。
从初始路标的邻域中随机选取n个新的路标，得到路标在目标天体坐标系下的位置矢量及轨道预报给出的深空探测器相对目标天体的大致位置矢量计算目标天体坐标系下n个路标相对探测器的位置矢量
令和分别为第i个与第j个路标的单位视线向量，其中：ri和rj为第i个与第j个路标到探测器的距离，对n个路标中第i个和第j个路标组合的观测矩阵的行向量进行计算
其中和为辅助向量，计算公式如下
其中，Aij为探测器到第i个和第j个路标观测视线所形成的观测角。
对于n个新路标，任意选取两个路标，两两组合后得到的观测矩阵行向量共有n(n-1)/2个值。观测矩阵H由n个新路标所对应的行向量构成，由观测矩阵H得到矩阵(HTH)-1，矩阵(HTH)-1始终是3×3矩阵。设矩阵(HTH)-1主对角线上的元素为akk(k＝1,2,3)，则n个新路标所对应的矩阵(HTH)-1特征值之和λ为
步骤6，计算特征值之和λ与λmin的差值dE，并判断差值dE是否小于0，根据判断结果执行步骤8或步骤7。
由步骤5得到新的矩阵(HTH)-1特征值之和λ，计算特征值之和λ与λmin的差值dE
dE＝λ-λmin (19)
若差值dE小于0，执行步骤8，否则执行步骤7。
步骤7，计算接受概率p，并判断接受概率p是否大于[0,1]区间内的随机数，根据判断结果执行步骤8或步骤9。
根据梅特罗波利斯Metropolis接受准则，计算接受概率p：
p＝exp(-(λ-λmin)/Tk) (20)
其中Tk为当前接受概率参数。
随机生成[0,1]区间内的数，若p大于该随机数，则执行步骤8，否则执行步骤9。
根据步骤7的判断，使步骤8选取的当前最优路标具有概率突跳特性。
步骤8，选取当前最优路标。
当步骤5随机选取新的路标满足步骤6判断差值小于0条件，则步骤5随机选取新的路标即为当前最优路标，当前最优路标对应的特征值之和λ为最小特征值之和，即令λmin＝λ。
当步骤5随机选取新的路标满足步骤7判断接受概率p大于[0,1]区间内随机数的条件，则步骤5随机选取新的路标即为当前最优路标，当前最优路标对应的特征值之和λ为最小特征值之和，即令λmin＝λ。
步骤9，判断迭代次数是否达到N。
判断结果为是，执行步骤10，判断结果为否，返回步骤5。
步骤10，判断是否满足步骤4所述的终止条件。
判断结果为是，执行步骤12，判断结果为否，执行步骤11。
步骤11，降低接受概率参数T。
依照接受概率参数T的更新函数降低接受概率参数T，接受概率参数T的更新函数为Tk+1＝αTk，其中k为当前迭代次数，Tk为当前接受概率参数，Tk+1为更新后的接受概率参数，α为常数，0<α<1，令α＝0.99。
更新接受概率参数Tk+1之后，返回步骤5。
步骤12，终止迭代，得到最小特征值之和λmin及其对应的最优路标。
迭代运算终止，得到最小特征值之和λmin，λmin所对应的路标即为选取的最优导航路标。
至此，完成行星着陆光学导航最优路标的选取，利用本发明方法选取3、4、5和6个最优路标，其效果图如图3所示。为了比较穷举法和本发明方法的运算速度，表1列出了穷举法和本发明方法解算最优路标的运行时间，其中运行本发明方法的电脑CPU为Intel Core i7-6700HQ，主频2.6GHz，软件环境为MATLAB。当导航路标数目较多且最优路标数目大于3个时，本发明方法能够快速地解算出最优路标。
表1选取不同数目最优路标时穷举法和专利方法的运行时间
还包括步骤13：在步骤12选取最优导航路标之后，在选取最优导航路标之后，利用轨道预报给出的着陆器当前位置先验估计值r*，确定测量角度的预报值以及线性向量hij，利用位置偏差量与测量夹角偏差量δAij之间的关系得到
δA＝Hδr (21)
其中δA是由第i个和第j个路标观测视线所形成的观测角的偏差δAij组成的矩阵，H是由第i个和第j个路标观测视线所形成的观测向量hij组成的矩阵。因而着陆器当前时刻的位置矢量为
r＝r*+(HTH)-1HTδA (22)
由于在探测器相机坐标系下，导航路标的位置表示为
将上式进行归一化，可得
其中，利用导航路标的像元、像线坐标能够表示为
在已知着陆器位置的情况下，利用多矢量定姿原理，确定着陆器相对于目标天体固联系的姿态转移矩阵最优解为
其中
利用上述的位置解算公式(22)和姿态解算公式(26)，根据最优导航路标的观测信息能够解算探测器的位置和姿态，实现行星着陆光学导航。
以上所述的具体描述，对发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。