0 引言
为实现行星表面10～100 m的高精度着陆，NASA和ESA从20世纪90年代末开始研究利用光学相机拍摄序列图像来辅助惯性测量单元的导航方法。例如，NASA的勇气号（Sprit）、机遇号（Opportunity）在着陆火星时通过连续拍摄3幅地面的序列图像并匹配其中的2个相同Harris特征点，估计出着陆器相对地面的水平速度[4]。ESA的罗塞塔号（Rosetta）在着陆67P / Churyumov - Gerasimenko彗星时利用序列图像和雷达信息的多源融合实现了导航和避障[5]。日本宇宙航空研究开发机构（Japan Aerospace Exploration Agency，JAXA）的小行星探测器隼鸟号（Hayabusa）将拍摄的人造特征图像回传至地面并估计水平速度[6]。
导航滤波器的计算复杂度随观测陆标数量的增加而增大，为在计算资源受限的着陆器上实现自主导航，应尽量减少观测的陆标数量。在观测少量陆标时为了获得最高导航精度，则需优选对导航精度贡献最大的陆标进行观测。通常用可观测度来衡量导航系统的性能，可观测度越大则利用测量信息求解出的系统状态精度越高，误差收敛速度越快[7]。用于描述导航系统可观测度的传统方法主要有奇异值法[8]、几何精度因子（GDOP）法[7]等。Huang等[9]利用Lie导数建立了可观测性矩阵，并用其条件数分析了陆标分布对导航精度的影响。Yu等[10]利用可观测矩阵的条件数作为可观测度指标优选陆标。Xu等[11]推导了线性协方差模型作为可观测度来优选陆标。Zhao等[12]使用Fisher信息矩阵的迹结合CR不等式来描述火星着陆过程中陆标的可观测度。Zhu等[13]通过比较不同陆标组合对应的可观测性矩阵特征值之和的大小来选择最优陆标。但是上述方法通常只在一个时刻内对陆标进行静态的优选，没有综合考虑着陆器的运动过程，会导致对相同陆标进行重复观测时只有第一次观测是最优的，而继续观测时可观测度会逐渐降低。
针对上述问题，本文提出一种针对多次观测的序列图像可观测度指标。首先建立了观测一个未知陆标时的单时刻可观测度，其次结合着陆器的运动规律构建了多时刻序列图像可观测度，证明了其为凸函数并给出了最小值点所在区间，最后设计了陆标观测策略，指导优选多时刻可观测度最高的陆标，并通过数值仿真与单时刻可观测度进行了对比，验证了多时刻序列图像可观测度的导航精度更高。
1 序列图像自主导航模型
1.1 连续时间状态方程
定义着陆坐标系{L}是原点在预定着陆点，${x_{\text{L}}} - {y_{\text{L}}}$ 平面固定在行星表面，zL轴指向天空的东北天坐标系；着陆器本体坐标系{B}固定在着陆器上，原点位于质心C，如图1 所示。
着陆器的动力学方程表示为[14]：
式中，r、v和a分别为着陆器在着陆坐标系{L}下的位置、速度和加速度；${\boldsymbol{q}}$ 为从着陆坐标系{L}到本体坐标系{B}的姿态四元数；${\boldsymbol{\omega }}{\text{ = }}{[\begin{array}{*{20}{c}} {{\omega _x}}&{{\omega _y}}&{{\omega _z}} \end{array}]^{\text{T}}}$ 为本体系{B}相对着陆坐标系{L}的旋转角速度在{B}中的表示；${\boldsymbol{\varOmega }}({\boldsymbol{\omega }}) = \left[ {\begin{array}{*{20}{c}} { - [{\boldsymbol{\omega }} \times ]}&{\boldsymbol{\omega }} \\ { - {{\boldsymbol{\omega }}^{\text{T}}}}&0 \end{array}} \right]$ ；$[{\boldsymbol{\omega }} \boldsymbol{\times} ]$ 为${\boldsymbol{\omega }}$ 的叉乘反对称矩阵，$ [{\boldsymbol{\omega }} \times ] = \left[ {\begin{array}{*{20}{c}} 0&{ - {\omega _z}}&{{\omega _y}} \\ {{\omega _z}}&0&{ - {\omega _x}} \\ { - {\omega _y}}&{{\omega _x}}&0 \end{array}} \right] $ ；$ {{\boldsymbol{b}}_a} $ 和${{\boldsymbol{b}}_\omega }$ 分别为加速度计和角速度计的漂移偏差；${{\boldsymbol{n}}_{wa}}$ 和${{\boldsymbol{n}}_{w\omega }}$ 分别为驱动$ {{\boldsymbol{b}}_a} $ 和${{\boldsymbol{b}}_\omega }$ 的零均值白噪声。
IMU的输出加速度和角速度分别为$ {{{a}}_{{\mathrm{imu}}}} $ 和$ {{\mathbf{\omega }}_{{\mathrm{imu}}}} $ ：
式中，$ {\boldsymbol{C}}({\boldsymbol{q}}) $ 为q对应的姿态转移矩阵；g为着陆系{L}下的重力加速度；$ {{\boldsymbol{\omega }}_i} $ 为着陆系{L}下的行星旋转角速度；$ {{\boldsymbol{n}}_a} $ 和$ {{\boldsymbol{n}}_\omega } $ 分别为零均值高斯白噪声形式的测量噪声。
由于着陆的行星旋转角速度$ {{\boldsymbol{\omega }}_i} $ 较小，为了简化后续推导，给出如下假设：
假设 1[15]：在着陆段内与行星旋转角速度$ {{\mathbf{\omega }}_i} $ 的相关项$ 2[{{\boldsymbol{\omega }}_i} \times ]{\boldsymbol{v}} $ 、$ {[{{\boldsymbol{\omega }}_i} \times ]^2}{\boldsymbol{r}} $ 以及$ {\boldsymbol{C}}({\boldsymbol{q}}){{\boldsymbol{\omega }}_i} $ 均为小量，可以忽略。
观测到的陆标可分为位置已知的陆标（下文简写为已知陆标）和位置未知的陆标（下文简写为未知陆标）。只观测未知陆标时，着陆器位置和偏航角不可观测[15]；当观测一个已知陆标和若干个未知陆标时，只有着陆器偏航角不可观测[16]。在未知环境中着陆时无法由地图匹配获得除着陆点以外的已知陆标，因此本文所观测的陆标中包含一个已知陆标。另外，Martinelli[16]通过实验发现在观测一个已知陆标和一个未知陆标的基础上，增加未知陆标数量不会明显提高导航精度，为了最大化地减小导航滤波器的复杂度，本文构建的序列图像导航滤波器只观测一个已知陆标${p_1}$ 和一个未知陆标${p_2}$ 。$ {{\boldsymbol{r}}_{p1}} $ 和$ {{\boldsymbol{r}}_{p2}} $ 分别为${p_1}$ 和${p_2}$ 在着陆坐标系{L}下的坐标。在着陆过程中除了需要估计着陆器状态以外，还需要估计未知陆标${p_2}$ 的位置$ {{\boldsymbol{r}}_{p2}} $ ，因此系统状态向量可定义为
1.2 离散时间状态方程
定义着陆器误差状态为$ {\boldsymbol{\tilde x}} = {\boldsymbol{x}} - {\boldsymbol{\hat x}} $ ，${\boldsymbol{x}}$ 的估计值是${\boldsymbol{\hat x}}$ ，误差状态向量为：
式中，$\delta {\boldsymbol{\theta }}$ 为姿态角误差。误差四元数定义为$\delta {\boldsymbol{q}} = {\boldsymbol{q}} \otimes {{\boldsymbol{\hat q}}^{ - 1}}$ ，当误差姿态角为小角度时，姿态角误差与误差四元数的关系为$\delta {\boldsymbol{q}} \approx {\left[ {\begin{array}{*{20}{c}} {\delta {{\boldsymbol{\theta }}^{\text{T}}}/2}&1 \end{array}} \right]^{\text{T}}}$ 。$ {\tilde{\boldsymbol{x}}} $ 从第i到第j个采样时刻的离散时间状态方程可表示[15]为：
式中，i为第i个采样时刻；（j,i）为从第i到第j个采样时刻；$ \Delta t $ 为采样周期；$ \boldsymbol{I}_3\in\mathbf{R}^{3\times3} $ 为3×3的单位矩阵；$ 0_{3\times3}\in\mathrm{\mathbf{R}}^{3\times3} $ 为3×3的0矩阵；${t_i}$ 为第i个采样时刻对应的时间。
1.3 测量方程
第k个采样时刻的测量值是所有观测到的陆标相对着陆器的方向在{B}中的表示：
式中，${{\boldsymbol{r}}_i}$ 为着陆系{L}下陆标${p_i}$ 相对着陆器的位置；${{\boldsymbol{\tilde z}}^{(k)}}{\text{ = }}{{\boldsymbol{z}}^{(k)}} - {{\boldsymbol{\hat z}}^{(k)}}$ 为${{\boldsymbol{z}}^{(k)}}$ 关于估计值${{\boldsymbol{\hat z}}^{(k)}}$ 的误差，因此测量方程可以线性化表示为：
其中
式中，$ {{\boldsymbol{\eta }}^{(k)}} $ 为测量噪声，利用扩展卡尔曼滤波器对状态方程(5)和测量方程(11)进行滤波即可同时估计着陆器导航状态与未知陆标的位置。
2 基于序列图像可观测度的特征优选方法
2.1 单时刻可观测度
为了规划未知陆标${p_2}$ 在序列图像中的最优位置，先构建单时刻下描述陆标位置估计误差的可观测度。在行星着陆过程中，一般陆标的高度远比着陆器的高度小，为了方便指标建立，给出如下假设。在假设2条件下对陆标${p_2}$ 位置进行规划，从序列图像中提取相应位置处的陆标，但在导航滤波器中依旧估计其三维位置。
假设2：未知陆标${p_2}$ 处于着陆系{L}的平面${x_{\text{L}}} - {y_{\text{L}}}$ 内。
在假设2条件下，设计未知陆标${p_2}$ 的可观测度时只需考虑${x_{\text{L}}}$ 和${y_{\text{L}}}$ 方向的位置估计精度。因此，测量方程中与${{\boldsymbol{r}}_{p2}}$ 对应的项被重新写为
式中，r(k)的逆${\left( {{\gamma ^{(k)}}} \right)^{ - 1}}$ 为描述k时刻对陆标${p_2}$ 位置估计精度的单时刻可观测度模型，$\hat r_z^{(k)} = [\begin{array}{*{20}{c}} 0&0&1 \end{array}]{\boldsymbol{\hat r}}_{}^{(k)}$ 。
当${\left( {{\gamma ^{(k)}}} \right)^{ - 1}}$ 越大时，对未知陆标${p_2}$ 的位置估计精度越高。由于$\hat r_z^{(k)}$ 的值等于着陆器高度的估计值，因此陆标到着陆器的距离${\left\| {{\boldsymbol{\hat r}}_2^{(k)}} \right\|_2}$ 越近时，单时刻可观测度${\left( {{\gamma ^{(k)}}} \right)^{ - 1}}$ 越大。
2.2 序列图像可观测度
2.1节建立的单时刻可观测度模型适用于在一个观测时刻内优选陆标位置的传统思路，为了使未知陆标${p_2}$ 的位置估计精度最高，只需最小化式(18)获得使单时刻可观测度最大的陆标位置并进行观测。
引理1[17]：在未知环境中，对着陆系原点处陆标和至少1个未知陆标最少观测4次，可使除偏航角外的状态开始收敛。
但是，根据引理1，至少需要对相同陆标观测4次。如果在第一次观测陆标时选择了当前时刻可观测度最大陆标作为导航陆标，为了满足引理1的最少观测次数，需要飞行一段距离后继续观测相同的陆标，此时该陆标的可观测度已开始降低，且随着着陆器的向前飞行，陆标的可观测度不断降低，使得连续4次观测内的整体陆标可观测度相对较低，影响导航精度的提升。
为解决这一问题，如果能综合考虑观测4次陆标时的序列图像可观测度，找到综合可观测度最大的陆标并对其进行连续4次观测，就能在着陆器下降过程中获得更高的导航精度。在假设2条件下，陆标位置${{\boldsymbol{r}}_{p2}}$ 在4次观测的测量方程中的对应项为：
因此，可以根据制导律${\boldsymbol{\hat a}}$ 给出的运行轨迹计算4次观测陆标时的序列图像可观测度，恰好是从k到k+3这4个单时刻可观测度之和：
式中：$ J^{(k + 3,k)} $ 的逆${\left( {{J^{(k + 3,k)}}} \right)^{ - 1}}$ 为描述从k到k+3时刻陆标${p_2}$ 位置估计精度的序列图像可观测度模型，当${\left( {{J^{(k + 3,k)}}} \right)^{ - 1}}$ 越大时，对未知陆标${p_2}$ 的位置估计精度越高。
2.3 序列图像陆标优选方法
优化式(20)就能获得考虑4次观测时序列图像可观测度最大的新陆标：
定理1：式(24)所优化的目标函数(20)是关于陆标位置${{{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{\boldsymbol{r}} }}_{p2}}$ 的凸函数。
证明：
由于$ {\boldsymbol{\hat r}}_{}^{(k + s)} $ 和$ \hat r_z^{(k + s)} $ 可由式(23)的制导律计算得到，$ {J^{(k + 3,k)}} $ 是关于${{{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{\boldsymbol{r}} }}_{p2}}$ 仿射变换的范数的偶数次幂之和的函数，因此$ {J^{(k + 3,k)}} $ 是关于陆标位置${{{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{\boldsymbol{r}} }}_{p2}}$ 的凸函数。
根据定理1的结论，利用凸优化算法就能按照式(24)优化$ {J^{(k + 3,k)}} $ 这一凸函数，从而快速优选出可观测度最高的导航陆标位置${{\boldsymbol{r}}_{p2}}$ 的唯一解。
定理2：式(24)的最小值点在如下区间内：
式中：${{\boldsymbol{\hat r}}^{(k + s)}} = {\left[ {\begin{array}{*{20}{c}} {\hat r_x^{(k + s)}}&{\hat r_y^{(k + s)}}&{\hat r_z^{(k + s)}} \end{array}} \right]^{\text{T}}}$ 。
证明：
目标函数$ {J^{(k + 3,k)}} $ 可展开为：
对陆标位置${{{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{\boldsymbol{r}} }}_{p2}}$ 求偏导得到：
分别计算$ {J^{(k + 3,k)}} $ 在${{\boldsymbol{r}}_{p2}}$ 取值区间上下界$ {{{\underset{\raise0.3em\hbox{$\smash{\scriptscriptstyle-}$}}{\boldsymbol{r}} }}^{(k + 3,k)}} $ 和$ {{\boldsymbol{\bar r}}^{(k + 3,k)}} $ 处的偏导数：
根据定理1的结论，由于$ {J^{(k + 3,k)}} $ 是凸函数，并且在区间下界$ {{{\underset{\raise0.3em\hbox{$\smash{\scriptscriptstyle-}$}}{\boldsymbol{r}} }}^{(k + s,k)}} $ 处导数为负，在区间上界$ {{\boldsymbol{\bar r}}^{(k + s,k)}} $ 处导数为正，因此$ {J^{(k + 3,k)}} $ 的最小值点在区间$ {{{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{\boldsymbol{r}} }}_{p2}} \in \left( {{{{{\underset{\raise0.3em\hbox{$\smash{\scriptscriptstyle-}$}}{\boldsymbol{r}} }}}^{(k + 3,k)}},{{{\boldsymbol{\bar r}}}^{(k + 3,k)}}} \right) $ 内。
根据定理2的结论，每次优选新陆标时，只需要在区间$ {{{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{\boldsymbol{r}} }}_{p2}} \in \left( {{{{{\underset{\raise0.3em\hbox{$\smash{\scriptscriptstyle-}$}}{\boldsymbol{r}} }}}^{(k + 3,k)}},{{{\boldsymbol{\bar r}}}^{(k + 3,k)}}} \right) $ 内优化函数$ {J^{(k + 3,k)}} $ 来获得新的陆标位置，搜索范围有限，有利于快速获得最优值点。仅用单时刻可观测度优选的陆标位于k时刻着陆器在地面的投影处$ {{{\underset{\raise0.3em\hbox{$\smash{\scriptscriptstyle-}$}}{\boldsymbol{r}} }}^{(k + 3,k)}} $ ，是定理2给出的最优值存在区间边界。当关注k到k+3 4个时刻的可观测度总和时，根据定理1和定理2，序列图像可观测度最大的陆标一定优于单时刻可观测度优选的陆标。因此，相比基于单时刻可观测度的传统陆标优选思路，采用序列图像可观测度优选的陆标能提供长时间段内更高的综合导航精度。
基于序列图像可观测性分析的自主导航陆标观测策略是：通过优化式(20)优选出新陆标后，进行状态递推和陆标观测，利用卡尔曼滤波器估计导航状态，当旧陆标观测次数满4次后重新优选新陆标，观测策略流程如图2 所示。
3 仿真校验
本节仿真模拟着陆器着陆于火星表面的过程，以“天问一号”着陆火星为例，着陆点为109.9°E、25.1°N。着陆器下降的轨道由多项式制导律生成，着陆过程持续198 s，导航滤波的采样周期$ \Delta t=1\mathrm{\ s} $ ，陆标观测间隔10 s，陆标均匀分布在火星表面。着陆器的初始位置和速度由地球上的深空测控网确定，初始姿态由轨道器在火星轨道上运行时通过姿态确定和控制系统（Attitude Determination and Control System，ADCS）自主确定。着陆器在着陆坐标系下的初始状态见表1 。IMU的加速度测量噪声标准差为$5 \times {10^{{\text{ - 4}}}}$ m/s2，角速度测量噪声标准差为$2.78 \times {10^{{\text{ - 5}}}}$ （°）/s。采用蒙特卡洛仿真时仿真次数为300次，着陆器初始状态误差在表2 所给出的误差界内均匀分布。
表 1 着陆器初始状态Table 1 Initial state of lander |
| 状态参数 | 初始数值 |
| 位置/m | [ |
| 速度/m·s-1 | [−56 −66 −90]T |
| 姿态/（ | [−25.7 −9.8 0]T |
表 2 蒙特卡洛仿真初始状态误差Table 2 Initial state error during Monte Carlo simulation |
| 状态参数 | 初始状态误差界 |
| 位置/m | |
| 速度/m·s-1 | 10 |
| 姿态/（ | 2 |
着陆过程的真实位置、速度、姿态变化曲线如图3 所示，可以看出着陆器的位置和速度根据制导律逐渐收敛至坐标系原点成功着陆。
为了探究陆标位置与可观测度的关系，通过仿真计算每个陆标的可观测度，如图4 所示。图4 展示的可观测度随陆标位置的变化符合定理1所述的凸函数特征，可以利用黄金分割法对该模型进行寻优，优选对导航精度贡献最高的陆标。根据4个观测时刻着陆器的位置给出了陆标位置的寻优上下界，可观测度最大的陆标恰好处于该区域之中，验证了定理2的正确性。
以着陆开始的前4次观测为例对比序列图像可观测度相比单时刻可观测度的优势。在k=1时刻通过单时刻可观测度优选的陆标位于${\boldsymbol{r}}_{p2}^1$ ，通过序列图像可观测度优选的陆标位于${\boldsymbol{r}}_{p2}^*$ 。计算连续4次观测的单时刻可观测度${\left( {{\gamma ^{(k)}}} \right)^{ - 1}}$ ，如表3 所示，可以看出，虽然k=1时刻${\boldsymbol{r}}_{p2}^1$ 处陆标比${\boldsymbol{r}}_{p2}^*$ 处陆标的可观测度更高，从k=2到k=4时刻${\boldsymbol{r}}_{p2}^*$ 处陆标的可观测度更高，并且综合4个观测时刻的总可观测度也是${\boldsymbol{r}}_{p2}^*$ 处陆标更高。这是由于序列图像可观测度通盘考虑了多次观测的综合可观测度，即使在第一次观测时损失了部分导航精度，但在后续观测中能获得更高精度。
表 3 单时刻与序列图像可观测度对比Table 3 Initial state error during Monte Carlo simulation |
| k | 1 | 2 | 3 | 4 | 总和 |
在定理2给出的区域中优选序列图像可观测度最大的陆标，连续观测4次后重新优选新陆标，这一观测策略的效果在图5 中直观展示。
通过蒙特卡洛仿真将本文提出的序列图像可观测度与传统单时刻可观测度进行对比。仿真300次后的导航误差如图6 所示，由导航误差3σ包络线可以看出，采用本文提出的序列图像可观测度进行陆标优选比传统单时刻可观测度的位置误差收敛更快，导航精度更高，表明序列图像可观测度能综合考虑多时刻的观测，提升整体着陆过程中的导航精度。
4 结 语
针对行星着陆过程中序列图像自主导航的陆标优选问题，提出了一种针对多时刻的基于序列图像可观测度的陆标优选方法，在单时刻可观测度的基础上，结合着陆器运动规律，建立了序列图像可观测度模型，并证明其为凸函数，也给出了寻优的区间，在此区间内通过凸优化就能快速优选出导航精度贡献最高的陆标位置。通过数学仿真验证了本文提出的基于序列图像可观测度的陆标优选方法比传统单时刻优选的导航精度更高，导航误差收敛更快。