CN114577205A - 一种基于序列图像的行星软着陆自主导航陆标优选方法 - Google Patents

Abstract

一种基于序列图像的行星软着陆自主导航陆标优选方法，属于航天器导航制导控制技术领域，包括以下步骤：S1、着陆器进行着陆时采用视觉辅助惯性导航方法，建立视觉辅助惯性导航方法的离散时间的状态和观测误差方程；S2、根据S1建立的误差方程，判断离散时间系统可观测性矩阵的秩，分析保证可观测状态收敛条件下，陆标的最少观测次数；S3、构建可观测度指标模型；可观测度指标模型用于表征着陆器观测到陆标时对应着陆器位置的估计误差；S4、利用S2所述最少观测次数的结论及S3所述可观测度指标模型构建观测策略，指导着陆过程中自主切换陆标。

Description

一种基于序列图像的行星软着陆自主导航陆标优选方法

技术领域

本发明涉及一种基于序列图像的行星软着陆自主导航陆标优选方法，属于航天器导航制导控制技术领域。

背景技术

2020年，中国和美国先后发射了火星探测器并实施着陆任务，其中，高精度自主导航是保障着陆器顺利精确定点着陆的关键。惯性测量单元(IMU)是探测器携带的基本自主导航器件，但由积分带来的导航误差累积是惯性导航缺陷。因此常用结合外部测量设备来提高导航精度。

为实现10至100米级别的行星精确着陆，NASA和ESA从上世纪末开始研究基于视觉的相对导航技术。将序列图像辅助惯性测量单元的导航方法利用在着陆过程中是可行的，并且能够获得高精度的导航结果。这种基于序列图像和惯性测量单元组合的自主导航方法通常被称为视觉辅助惯性导航(VAIN)。

目前针对视觉辅助导航的可观测性研究已取得一定进展，但仍然存在以下不足：

(1)已有论文都是针对特定观测条件的，缺乏对相同陆标观测次数的讨论。如果能分析出不同观测方案中的可观测性，就能指导工程实践中选择更好的观测方案，获得满足可观测需求的边界条件。观测所需的最低次数是保证导航系统收敛的必要条件，可以用于指导观测策略的设计。

(2)关于VAIN问题可观测性的研究主要集中于可观测的性质，用于探究系统的不可观测方向，但用于描述导航系统精度的可观测度的研究较少。但已有的研究方法仅适用于飞行前设计数据库或着陆后分析滤波过程，没法做到着陆过程中在线评估并指导陆标选取。在着陆阶段内，着陆器受到计算能力的限制，如果能通过在线计算可观测度来选择对导航精度贡献更大的陆标，就能在尽可能观测少量陆标的情况下获得较高导航精度。

发明内容

本发明要解决的技术问题是：克服现有技术的不足，系统地分析视觉辅助惯性导航方法的可观测性，设计一种用于可观测度指标和观测策略，用于指导着过程中自主规划陆标。

本发明目的通过以下技术方案予以实现：

一种基于序列图像的行星软着陆自主导航陆标优选方法，包括如下步骤：

S1、着陆器进行着陆时采用视觉辅助惯性导航方法，根据视觉辅助惯性导航方法的连续时间状态方程和观测方程，确定着陆器观测陆标时的离散时间的状态误差方程和观测误差方程；

S1确定离散时间的状态误差方程的方法，具体如下：

根据视觉辅助惯性导航的连续时间状态方程，确定连续时间误差状态方程，从而确定离散时间的状态误差方程；

式中：

表示系统的误差状态矢量，

表示着陆器的状态误差，

表示陆标位置的状态误差；

表示系统状态转移矩阵，n_s表示位置已知陆标(后文简写为已知陆标)的个数，n_p表示位置未知陆标(后文简写为未知陆标)的个数；右上角标(i)表示第i个时刻，(j,i)表示从第i到第j时刻；

Δt表示离散时间间隔，

r、v表示着陆器在着陆坐标系下的位置、速度矢量； g表示着陆系下的重力加速度；q表示从着陆坐标系到本体坐标系的姿态四元数；C(q)表示q对应的姿态转移矩阵；

和

表示IMU输出的加速度和角速度；t_i,t_j分别表示第i和第j时刻；对于任意三维矢量a＝[a_x a_y a_z]^T，

确定离散时间的观测误差方程的方法，具体如下：

根据观测方程，确定观测误差方程；

式中：

表示观测误差矢量；η^(k)表示测量噪声；

r_i表示从着陆器到第i个陆标的矢量。

S2根据着陆器观测陆标时的离散时间的状态误差方程和观测误差方程，获得离散时间系统可观测性矩阵，判断离散时间系统可观测性矩阵的秩是否满秩，若满秩则进入S5，反之则当前观测次数累加1并进入步骤S3；

S3增加一个采样时刻对应的离散时间的状态误差方程和观测误差方程，并进入步骤S4；

S4更新离散时间系统可观测性矩阵，判断离散时间系统可观测性矩阵的秩是否满秩，若满秩则进入S5反之则当前观测次数累加1并返回S3直至离散时间系统可观测性矩阵的秩满秩；

S5获得当前的观测次数n；

S6构建可观测度指标模型；可观测度指标模型用于表征着陆器观测到陆标时对应着陆器位置的估计误差；

所述可观测度指标模型，具体为：

J＝δr+δλ

假设C为着陆器质心，p₁为着陆系原点，p₂和p₃为水平面x_L-y_L内的待优选的陆标，并且p₂和p₃是关于平面Cp₁z_L对称的；r是从p₁到C的矢量，r_p2是从p₁到 p₂的矢量，r₁是从C到p₁的矢量，r₂是从C到p₂的矢量；式中：α表示r与r_p2之间的夹角，β表示r₁与r₂之间的夹角，γ表示r₂与r₃之间的夹角；l表示C到p₁p₂的距离；ε表示姿态估计误差界；

表示观测上一个陆标时的位置估计误差；μ表示扩张参数。

S7求取能够使所述估计误差最小的两个观测角(β，γ)；根据S5获得的观测次数n和获得的两个观测角(β，γ)，获得着陆器与需要进行观测陆标之间的两个方向矢量，并获得着陆器的位置、速度和姿态角。

所述选取着陆器需要进行观测的陆标的方法，具体为：

(1)优化求解J并获得能够使可观测度指标模型值最小的两个观测角β^*(k)和γ^*(k)，取β＝β^*(k)，γ＝γ^*(k)，根据两个观测角获得着陆器与需要进行观测陆标之间的两个方向矢量以及着陆器与基准陆标之间的方向矢量；

(2)根据步骤(1)获得的三个方向矢量，对离散时间的状态误差方程和观测误差方程进行卡尔曼滤波n个周期，获得着陆器的位置、速度和姿态角；

(3)当下一观测时刻到来后，由于着陆器位置改变，需要重新优化求解J 并获得β^*(k)和γ^*(k)，判断J(β^*(k),γ^*(k),μ＞0)与J(β^(k,k-1),γ^(k,k-1),μ＝0)的大小，如果 J(β^*(k),γ^*(k),μ＞0)<J(β^(k,k-1),γ^(k,k-1),μ＝0)，则执行(4)，否则执行(5)。

(4)取β＝β^*(k)，γ＝γ^*(k)，观测β和γ对应的陆标，根据两个观测角获得着陆器与需要进行观测陆标之间的两个方向矢量以及着陆器与基准陆标之间的方向矢量，根据步骤(1)获得的三个方向矢量，对离散时间的状态误差方程和观测误差方程进行卡尔曼滤波n个周期，获得着陆器的位置、速度和姿态角，并返回(3)；n的取值范围为1～10；

(5)继续观测旧的陆标，滤波1个周期，获得着陆器的位置、速度和姿态角，并返回(3)。

本发明相比于现有技术具有如下有益效果：

(1)针对视觉辅助惯性导航的可观测性问题，已有研究成果大多集中于不可观测状态的分析，缺乏针对最少观测次数的研究。针对计算资源严格受限的着陆器，着陆过程中拍摄图像占用资源越少越好，本发明系统地讨论了观测不同陆标个数时保证可观测状态收敛的最少观测次数。最少观测次数的研究成果能指导观测策略的设计，约束着陆器从序列图像中提取陆标作为导航路标时，对同一陆标的最少观测次数，从而保证着陆器状态估计值收敛。

(2)针对视觉辅助惯性导航的可观测度问题，已有研究成果大多仅适用于飞行前设计陆标识别数据库或着陆后分析滤波精度，难以用于在线评估陆标对导航精度的贡献程度。本发明设计的可观测度指标仅为单变量初等函数，适用于着陆过程中自主规划，可以用于指导在限制陆标观测个数条件下陆标的选择，从而提高导航精度。

附图说明

图1为观测几何示意图；

图2为平面Cp₁p₂示意图；

图3为平面Cp₂p₃示意图；

图4(a)为着陆过程中着陆下降轨迹的自主导航仿真示意图；

图4(b)为着陆过程中陆标观测次数的自主导航仿真示意图；

图5(a)为不同陆标个数时位置误差的蒙特卡洛仿真图；

图5(b)为不同陆标个数时速度误差的蒙特卡洛仿真图；

图5(c)为不同陆标个数时姿态误差的蒙特卡洛仿真图；

图6(a)为不同可观测度为位置误差时的指标对比图；

图6(b)为不同可观测度为优化算法消耗时间时的指标对比图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明的实施方式作进一步详细描述。

实施例1：

一种基于序列图像的行星软着陆自主导航陆标优选方法，包括如下步骤：

(1)建立视觉辅助惯性导航的状态方程和观测方程；首先，视觉辅助惯性导航的连续时间状态方程为：

式中：r、v表示着陆器在着陆坐标系下的位置、速度矢量；

表示r、v的导数；q表示从着陆坐标系到本体坐标系的姿态四元数；C(q)表示q对应的姿态转移矩阵；g表示着陆系下的重力加速度；

和

表示IMU输出的加速度和角速度；

b_a和b_ω分别是加速度计和角速度计的漂移偏差；n_wa和n_wω表示漂移偏差噪声。

定义

和

分别是r和v的误差，δθ表示姿态角误差。误差状态矢量就表示为

因此着陆器误差状态的连续时间误差状态方程表示为：

式中：

表示

的导数；系统噪声定义为

n_a、n_ω分别表示IMU 的加速度漂移噪声和角速度漂移噪声；

0_3×3∈R^3×3表示3×3的零矩阵；I₃∈R^3×3表示3×3的单位矩阵。假设观测到n_s个位置已知的陆标(后文简写为已知陆标)和n_p个位置未知的陆标(后文简写为未知陆标)，r_pi表示第i个陆标p_i在着陆坐标系{L} 下的坐标，

是已知陆标，

是未知陆标。在着陆过程中除了需要估计着陆器状态以外，还需要估计未知陆标在{L}中的位置，因此未知陆标的位置状态矢量表示为

陆标位置的连续时间状态方程为

式中：

表示x_p的导数。

待估计的状态是着陆器状态与陆标位置的组合

从i时刻到j 时刻的离散时间状态误差方程表示为

式中：右上角标(i)表示第i时刻，(j,i)表示从第i到第j时刻；着陆器状态

的状态转移矩阵

为

式中：Δt表示离散时间间隔；

设r_i表示着陆系{L}下由着陆器指向陆标p_i的矢量，测量量^be_i是r_i旋转到本体系{B}后的单位矢量，因此观测方程为

k时刻的测量量为观测每个陆标的方向矢量

z^(k)的估计值是

则z^(k)的测量误差为

因此离散时间的观测误差方程为

式中：η^(k)表示测量噪声；

(2)根据离散时间系统可观测性矩阵的秩，分析可观测状态收敛的最少观测次数。

在不考虑噪声的情况下，根据离散时间系统的误差方程：

可以从任意k时刻开始观测N次后，系统的可观测性矩阵表示为

当带入真实状态x的

满秩时系统可观测。因为

是满秩的，

满秩等价于

满秩。

情况1：n_s＝0，n_p≥1

系统存在4个不可观测方向，需要判断可观测性矩阵的秩何时达到 (15+3n_p)-4＝11+3n_p。当观测次数N＝2时，

经过行列变换变为

可以看出

不满足秩条件。当观测次数增加到N＝3时，

经过行列变换变为

可以看出

未达到秩条件。当观测次数增加到 N＝4时，

经过行列变换变为

当N＝4且n_p＝1时，

未达到秩条件。当N＝4且n_p＞1 时且存在姿态变化时，

此时

右零空间的维数是4，对应了系统的4个不可观测方向，即三轴位置和偏航角。同时，除了这4个不可观测方向以外，不存在其他不可观测方向。说明在只观测至少2 个未知陆标的情况下，至少连续观测4次之后能使系统除三轴位置和偏航角以外的状态全可观。

当观测次数增加到N＝5且n_p＝1时，

经过行列变换变为

经高斯消元法，

因此

此时

右零空间的维数是4，对应了系统的4个不可观测方向，即三轴位置和偏航角。说明在只观测1个未知陆标的情况下，至少连续观测5次之后能使系统除三轴位置和偏航角以外的状态全可观。

情况2：n_s＝1，n_p≥0

存在1个不可观测方向，需要判断可观测性矩阵的秩何时达到 (15+3n_p)-1＝14+3n_p。当观测次数N＝2时，

经过行列变换变为

可以看出

不满足秩条件。当观测次数增加到 N＝3时，

经过行列变换变为

可以看出

未达到秩条件。当观测次数增加到 N＝4时，

经过行列变换变为

当N＝4且n_p＝0时，

未达到秩条件。当N＝4，n_p＞0且姿态存在姿态变化时，

此时

右零空间的维数是1，对应了系统的不可观测方向，即偏航角。同时，除了这个不可观测方向以外，不存在其他不可观测方向。说明在观测1个已知陆标和若干个未知陆标的情况下，至少连续观测4次之后能使系统除偏航角以外的状态全可观。

当观测次数增加到N＝5且n_p＝0时，

经过行列变换变为

可以看出

此时

右零空间的维数是1，对应了系统的不可观测方向，即偏航角。说明在只观测1个已知陆标的情况下，至少连续观测5次之后能使系统除偏航角以外的状态全可观。

情况3：n_s＞1，n_p≥0

当所有已知陆标间的连线都与g平行时，存在1个不可观测方向：偏航角，需要判断可观测性矩阵的秩何时达到(15+3n_p)-1＝14+3n_p；当存在已知陆标间连线不与g平行时全状态可观测，需要判断可观测性矩阵的秩何时达到15+3n_p。当观测次数N＝2时，

经过行列变换变为

可以看出

不满足秩条件。当观测次数增加到 N＝3时，

经过行列变换变为

因为

当所有已知陆标间的连线都与g平行时，系统的不可观测方向是偏航角。此时

右零空间的维数是1，对应了系统的不可观测方向。说明在观测至少2个已知陆标和任意个未知陆标，并且所有已知陆标间的连线都与g平行的情况下，至少连续观测3次之后能使系统除偏航角以外的状态全可观。如情况1.4中提到的，可以忽略这种陆标的分布情况。当存在2个已知陆标间的连线不与g平行时，系统没有不可观测方向。此时恰好

是满秩的。说明在观测至少2个已知陆标和任意个未知陆标，并且存在2个已知陆标间的连线不与g平行的情况下，至少连续观测3次之后能使系统的状态全可观。

综合上述3种情况的离散系统可观测性分析，以及4种情况的连续系统可观测性分析结果，可以得到如下结论：

(3)针对未知环境，本发明设计了可观测度指标，在限制观测3个陆标的条件下，指导着陆器自主跟踪拍摄可观测度最高的陆标。

系统可观测只能说明系统状态向真值收敛，但无法衡量估计误差的大小。在着陆器上计算资源受限的情况下，期望的目标是在跟踪更少的陆标时，依然能实现较高的导航精度。因此在陆标个数有限的情况下，需要合理规划选取所观测的陆标，提升导航精度。可观测度是描述了系统收敛速度和精度的重要指标，可观测度越大则导航精度越高。本章将构建着陆器VAIN导航系统的可观测度。

首先给出如下假设：

假设：在陆标规划过程中，未知陆标都处于着陆系的X-Y平面内。

在着陆环境未知的情况下，着陆器需要自主规划待观测的陆标。当陆标的状态具有三维坐标时，很难对三维未知空间进行陆标的优化选取；而在着陆器飞行高度较高时，如果认为陆标的高度与着陆器飞行高度相比小很多，将火星表面看作水平面，陆标都分布在水平面上。因此，本发明基于假设设计一个在水平面内规划陆标的方法，用于指导着陆器在三维空间内的观测及滤波。

基于可观测能性分析，在环境未知的着陆条件下，如果仅观测未知陆标，位置和偏航都不可观测。而如果将着陆系{L}的原点建立在着陆点处并持续观测着陆点，这样着陆点可作为着陆系中的已知陆标，就能使位置可观。但是在未知环境中无法观测到2个已知陆标。下面讨论仅观测3个陆标的观测模式，即在观测着陆点作为已知陆标p₁的基础上，额外观测2个未知陆标p₂、p₃。在此观测模式下设计可观测度指标，用于规划待观测的陆标。

如果只关心着陆器位置估计误差，观测矩阵取着陆器位置状态相关项：

传统的可观测度指标往往是观测矩阵的函数。在已知着陆器位置、姿态估计值的情况下H_r1是固定的，H_r2是未知陆标位置r₂的函数，H_r3是未知陆标位置 r₃的函数。可以看出传统可观测度指标都是r₂和r₂的函数，说明如果能合理规划未知陆标与着陆器的相对位置关系，就能增加可观测度，从而提高导航精度。

将着陆器位置用C表示，着陆器与3个陆标间的关系如图1所示。在地面是平面的假设条件下，3个陆标都处于X-Y平面内，为了简化设计，假设p₂和p₃是关于平面Cp₁z_L对称的，四面体构型只与β和γ两个角度参数有关，下面将设计关于β和γ的可观测度指标。

平面Cp₁p₂和Cp₁p₃内的可观测度：

首先讨论平面Cp₁p₂和Cp₁p₃内的可观测度，因为p₂和p₃对称，所以下面以平面Cp₁p₂为例。如图2所示，当平面Cp₁p₂与平面x_Lp₁y_L的夹角给定，r与r_p2之间的夹角α就固定已知，r₁与r₂之间的夹角为β∈(0,π-α)。

由图2可以看出在此平面内，着陆器位置估计误差由姿态估计误差和p₂的位置误差共同造成。姿态估计误差界为

P是扩展卡尔曼滤波器的协方差矩阵。p₂的位置误差界首先继承了观测上一个陆标时的位置估计误差

同时考虑到着陆器状态递推误差和^be₂的测量噪声带来新的误差，k时刻p₂的位置误差界

μ表示扩张参数。

用沿r₁方向的位置误差δr作为可观测度指标。当平面Cp₁p₂与平面x_Lp₁y_L的夹角给定时，平面Cp₁p₂与平面x_Lp₁y_L的交线p₁p₂固定，此时α固定已知，且p₂在直线p₁p₂上移动。将δr分为两部分考虑，第一部分是

在r₁方向上的投影，表示为

第二部分是观测p₂时姿态误差的影响，表示为

因此得到描述着陆器在r₁方向上位置估计误差的可观测度指标δr：

平面Cp₂p₃内的可观测度：

接着讨论平面Cp₂p₃内的可观测度。如图3所示，当平面Cp₂p₃与平面x_Lp₁y_L的夹角给定时，平面Cp₂p₃与平面x_Lp₁y_L的交线p₂p₃固定，且C与p₂p₃的距离l 固定已知。p₂和p₃在p₂p₃上关于l对称移动，设r₂和r₃的夹角为2γ∈(0,π)。p₂和p₃在直线p₂p₃上的位置误差界依然为δr_p，由姿态误差造成的角度误差界仍然为 2ε。因此在平面Cp₂p₃内，着陆器的位置误差由p₂和p₃的位置误差和着陆器姿态误差造成。这个误差范围构成一个四边形，利用它水平方向和竖直方向的对角线长度，δh和δl，作为平面Cp₂p₃内的可观测度。

在平面Cp₂p₃内的水平方向上，由陆标位置误差造成的着陆器水平方向上的误差就是

在给定l时||r₂||＝l/cosγ，因此

着陆器姿态误差形成的扇形弧段在水平方向上的影响是

因此水平方向上的误差δh表示为

在竖直方向上，陆标位置误差

的投影长度是

姿态误差形成的扇形弧段在竖直方向上的影响是

因此竖直方向上的着陆器位置估计误差δl可以表示为

为了使平面Cp₂p₃内的可观测度最大，需要让δh与δl的和最小，去掉常数项后平面内的可观测度指标表示为

δr和δλ就是本发明构建的三个面内的可观测度指标，用他们的和来表示着陆器着陆过程中总的可观测度指标，即J＝δr+δλ，他描述了同时观测3个陆标时的着陆器位置估计误差。

(4)构建了观测策略，利用S2所述最少观测次数的结论及S3所述可观测度指标指导着陆过程中自主切换陆标。

δr和δλ分别是β和γ的函数，通过优化β和γ可以找到让δr和δλ指标最小的观测方向，从而得到在可观测度指标J下的最优观测构型。但δr和δλ存在隐含的参数传递关系，如α和l，因此只能分别交替寻优。

已知量是着陆器位置估计

姿态误差界ε，上一时刻陆标位置误差界

每次要规划新的观测陆标时，

通过扩展卡尔曼滤波器的递推获得；ε由扩展卡尔曼滤波器的协方差矩阵获得

在首次滤波时是给定的初值

之后每次要规划时，由扩展卡尔曼滤波器的协方差矩阵获得

在每次观测之前，以J作为可观测度指标，寻找使J最小的β和γ。寻优流程是：

(1)在给定平面Cp₂p₃内，计算相应的l₍₁₎，寻找δλ的最小值点

(2)在给定平面Cp₁p₂和Cp₁p₃内，计算相应的α₍₁₎，寻找δr的最小值点

(3)重复(1)和(2)，直到

且

时停止，Tol表示收敛阈值。

第一次计算

时可以令平面Cp₂p₃垂直于

此时

z_v表示着陆器的高度。

针对观测到的陆标缺乏位置先验知识的未知环境，根据观测误差的几何关系，本发明设计了只观测3个陆标时的可观测度指标，可观测度指标用于指导着陆器自主跟踪拍摄可观测度最高的未知陆标；在J的可观测度指标下，当μ>0 时，优化J(β,γ,μ＞0)得到的β^*(k)和γ^*(k)是观测新陆标时最优构型的观测角度；如果继续跟踪观测上一时刻的陆标，则μ＝0，且β^(k,k-1)和γ^(k,k-1)可通过几何关系计算得到，此时可以直接计算出可观测度指标J(β^(k,k-1),γ^(k,k-1),μ＝0)的值。因为参数μ的取值变化，会使J(β^(k,k-1),γ^(k,k-1),μ＞0)>J(β^(k,k-1),γ^(k,k-1),μ＝0)，即使通过寻优能找到的β^*(k)和γ^*(k)能使观测新陆标时的J(β^*(k),γ^*(k),μ＞0)<J(β^(k,k-1),γ^(k,k-1),μ＞0)，但不一定能让J(β^*(k),γ^*(k),μ＞0)<J(β^*(k),γ^*(k),μ＞0)。因此通过比较J(β^*(k),γ^*(k),μ＞0)与 J(β^(k,k-1),γ^(k,k-1),μ＝0)的大小，就可以对比出观测新陆标与旧陆标对应的可观测度大小。如果旧陆标的可观测度大，则继续跟踪观测；如果新陆标能带来更大的可观测度，则切换到β^*(k)和γ^*(k)所对应方向上的新陆标。

在观测策略中为了保证可观测状态的收敛，根据最少观测次数的分析结果，观测一个新的陆标后需要连续观测至少4个滤波周期。

实施例2：

应用实施例1的方法，着陆坐标选取为天问一号着陆地点：109.9°E， 25.1°N。采用阿波罗落月时的着陆制导率作为着陆制导算法。着陆段持续198s。着陆器在着陆坐标系下的初始位置为[3300 5870 6570]^Tm，初始速度为 [-56 -66 -90]^Tm/s，初始姿态为[45 1800]^T°。首先验证特定初始条件下的观测结果，在观测着陆系原点和一个未知陆标时，设初始位置估计误差500m，初始速度估计误差3m/s，初始姿态角估计误差1°，视线角度测量误差0.01rad(1σ)。着陆过程如图4(a)(b)所示，着陆器的估计轨迹逐渐收敛到了真实值附近。红色星号表示每次滤波时观测的陆标。蓝色的点划线连接了着陆器和陆标，表示每个时刻的观测视线。可以看出陆标分布在下降轨迹两侧，在满足可观测性条件的前提下被频繁切换。在图4(b)中显示了观测每个陆标时的次数变化，验证了观测策略的有效性。

下面进行蒙特卡洛仿真验证，设初始位置估计误差范围±1000m，初始速度估计误差范围±10m/s，初始姿态估计误差范围±2°，服从均匀分布，仿真200 次后导航误差的3σ包络曲线如图5所示。图5(a)(b)(c)显示了观测不同陆标个数时的导航误差。

从图5可以看出，当n_s＝0且n_p＝1时，着陆器的位置和偏航不收敛；当n_s＝1 且n_p＝1时，着陆器的位置能够收敛，但偏航依然不收敛；当n_s＝2且n_p＝1时，全状态都能收敛了。

为了验证算法2提升导航精度的优越性，将算法2与误差椭球、几何精度因子及条件数等传统可观测度指标进行对比。仿真200次后导航误差的3σ包络曲线如图6(a)所示。

图6(a)显示了分别采用这三种传统可观测度指标和算法2中指标时的导航位置误差结果，可以看出采用算法2的指标后比其他指标的位置误差小，导航精度更高。

此外为了验证算法2在着陆器上在线应用的快速性，对比优化求解四种指标的时间消耗。仿真程序的运行环境是Matlab R2020a，Intel(R)Core(TM) i7-10750H CPU，16GBRAM。为了公平起见，在假设p₂和p₃关于平面Cp₁z_L对称前提下，利用四种指标在水平地面内规划陆标，依次交替规划β与γ，这样就都是单变量函数的规划问题，区别是各自的目标函数不同。并且对这四个规划问题都采用黄金分割法进行优化。每种指标时间消耗的分布情况如图6(b)所示。从上到下每条横线依次表示最大值，上四分位数，中位数，下四分位数，最小值。可以看出由于算法2中的可观测度指标比另外三种传统指标的算法复杂度更小，寻优时的计算效率更高，因此从求解快速性的角度明显优于另外三种方法，更易于在着陆器上在线应用。

综上所述，通过上述实施例，验证了本发明提出的一种基于序列图像火星着陆的3陆标自助规划方法的可行性和有效性。

本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。

本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。

Claims (8)

1.一种基于序列图像的行星软着陆自主导航陆标优选方法，其特征在于，包括如下步骤：

S1、着陆器进行着陆时采用视觉辅助惯性导航方法，根据视觉辅助惯性导航方法的连续时间状态方程和观测方程，确定着陆器观测陆标时的离散时间的状态误差方程和观测误差方程；

S2根据着陆器观测陆标时的离散时间的状态误差方程和观测误差方程，获得离散时间系统可观测性矩阵，判断离散时间系统可观测性矩阵的秩是否满秩，若满秩则进入S5，反之则当前观测次数累加1并进入步骤S3；

S3增加一个采样时刻对应的离散时间的状态误差方程和观测误差方程，并进入步骤S4；

S4更新离散时间系统可观测性矩阵，判断离散时间系统可观测性矩阵的秩是否满秩，若满秩则进入S5；反之则当前观测次数累加1并返回S3直至离散时间系统可观测性矩阵的秩满秩；

S5获得当前的观测次数n；

S6构建可观测度指标模型；可观测度指标模型用于表征着陆器观测到陆标时对应着陆器位置的估计误差；

S7求取能够使所述估计误差最小的两个观测角(β，γ)；根据S5获得的观测次数n和获得的两个观测角(β，γ)，获得着陆器与需要进行观测陆标之间的两个方向矢量，并获得着陆器的位置、速度和姿态角。

2.根据权利要求1所述的一种基于序列图像的行星软着陆自主导航陆标优选方法，其特征在于，所述确定离散时间的状态误差方程，具体如下：

根据视觉辅助惯性导航的连续时间状态方程，确定连续时间误差状态方程，从而确定离散时间的状态误差方程。

3.根据权利要求1所述的一种基于序列图像的行星软着陆自主导航陆标优选方法，其特征在于，所述确定离散时间的状态误差方程，具体如下：

式中：

表示系统的误差状态矢量，

表示着陆器的状态误差，

表示陆标位置的状态误差；

表示系统状态转移矩阵，n_s表示位置已知陆标的个数，n_p表示位置未知陆标的个数；右上角标(i)表示第i个时刻，(j,i)表示从第i到第j时刻；

Δt表示离散时间间隔；r、v表示着陆器在着陆坐标系下的位置、速度矢量；g表示着陆系下的重力加速度；q表示从着陆坐标系到本体坐标系的姿态四元数；C(q)表示q对应的姿态转移矩阵；

和

表示IMU输出的加速度和角速度；t_i,t_j分别表示第i和第j时刻；对于任意三维矢量a＝[a_x a_y a_z]^T，

4.根据权利要求3所述的一种基于序列图像的行星软着陆自主导航陆标优选方法，其特征在于，所述

中：

5.根据权利要求1所述的一种基于序列图像的行星软着陆自主导航陆标优选方法，其特征在于，所述确定离散时间的观测误差方程，具体如下：

根据观测方程，确定观测误差方程；

式中：

表示观测误差矢量；η^(k)表示测量噪声；

r_i表示从着陆器到第i个陆标的矢量。

6.根据权利要求2～5任意一项所述的一种基于序列图像的行星软着陆自主导航陆标优选方法，其特征在于，所述可观测度指标模型，具体为：

J＝δr+δλ

其中，r是从p₁到C的矢量，r_p2是从p₁到p₂的矢量，r₁是从C到p₁的矢量，r₂是从C到p₂的矢量；式中：α表示r与r_p2之间的夹角，β表示r₁与r₂之间的夹角，γ表示r₂与r₃之间的夹角；l表示C到p₁p₂的距离；ε表示姿态估计误差界；

表示观测上一个陆标时的位置估计误差；μ表示扩张参数；C为着陆器质心，p₁为着陆系原点，p₂和p₃为水平面x_L-y_L内的待优选的陆标，p₂和p₃关于平面Cp₁z_L对称。

7.根据权利要求6所述的一种基于序列图像的行星软着陆自主导航陆标优选方法，其特征在于，所述选取着陆器需要进行观测的陆标，具体为：

(1)优化求解J并获得能够使可观测度指标模型值最小的两个观测角β^*(k)和γ^*(k)，取β＝β^*(k)，γ＝γ^*(k)，根据两个观测角，获得着陆器与需要进行观测陆标之间的两个方向矢量以及着陆器与基准陆标之间的方向矢量；

(2)根据步骤(1)获得的三个方向矢量，对离散时间的状态误差方程和观测误差方程进行卡尔曼滤波n个周期，获得着陆器的位置、速度和姿态角；

(3)当下一观测时刻到来后，优化求解J并获得β^*(k)和γ^*(k)，判断J(β^*(k),γ^*(k),μ＞0)与J(β^(k,k-1),γ^(k,k-1),μ＝0)的大小，如果J(β^*(k),γ^*(k),μ＞0)<J(β^(k,k-1),γ^(k,k-1),μ＝0)，则执行步骤(4)，否则执行步骤(5)。

(4)取β＝β^*(k)，γ＝γ^*(k)，观测β和γ对应的陆标，根据两个观测角获得着陆器与需要进行观测陆标之间的两个方向矢量以及着陆器与基准陆标之间的方向矢量，根据步骤(1)获得的三个方向矢量，对离散时间的状态误差方程和观测误差方程进行卡尔曼滤波n个周期，获得着陆器的位置、速度和姿态角，并返回步骤(3)；

(5)继续观测旧的陆标，滤波1个周期，获得着陆器的位置、速度和姿态角，并返回步骤(3)。

8.根据权利要求7所述的一种基于序列图像的行星软着陆自主导航陆标优选方法，其特征在于，所述n的取值范围为1～10。

Images