基于未知陆标图像测量的自主相对导航方法研究

徐 超，黄翔宇，李茂登

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

引 言

随着深空探测技术的发展，越来越多的行星探测任务要求探测器能够在复杂未知地形环境下安全着陆，这就要求探测器具有在未知环境下进行自主相对导航的能力[1-2]。

传统的深空探测软着陆任务中主要采用惯性导航为主的自主导航方式，但惯性导航误差会随时间累积，因此需要利用其它敏感器测量对惯性导航进行修正。“嫦娥三号”（Chang'E-3，CE-3）和“嫦娥四号”（Chang'E-4，CE-4）软着陆采用了基于惯性导航配以测距测速修正的导航方法[3-4]。尽管测距测速敏感器可对惯性导航的高度和速度进行修正，但是其不能抑制导航系统水平位置误差的发散，可能导致着陆器偏离目标着陆点。

针对惯性导航配以测距测速修正的导航方法无法估计水平位置的问题，一种最为可行的解决方法是引入行星表面陆标图像信息进行自主导航。目前采用的陆标图像信息的地外天体着陆自主导航方法的研究大都是在已有着陆区域参考地形图的假设下进行的，通过在下降的过程中，将图像中提取的陆标与包含陆标位置信息的参考地形图进行匹配获得下降图像中导航陆标的位置信息，进而利用这些陆标的位置信息结合其图像测量信息得到探测器的导航信息[5]。然而，当事先没有着陆区域参考地形图或由于参考地形图与下降图像的分辨率差异过大导致无法进行特征匹配时，下降序列图像中观测和跟踪到的陆标位置信息将无法获取，需要解决在未知环境下利用未知位置信息的陆标进行导航的问题。

针对未知环境下的行星着陆导航问题，美国国家航空航天局（National Aeronautics and Space Administration，NASA）为“火星探测漫游者”（Mars Exploration Rover，MER）任务研究了下降图像运动估计子系统（Descent Image Motion Estimation System，DIMES），该系统通过跟踪3幅下降序列图像中的未知位置信息的特征点结合高度计测高信息来估计探测器相对着陆面的水平速度，通过实际的火星着陆任务验证了该系统的有效性[6-7]。然而，DIMES系统仅能估计探测器相对着陆面的水平速度信息。文献[8～9]利用同时定位与地图创建方法（Simultaneous Localization and Mapping，SLAM），将陆标位置作为系统状态进行估计从而实现飞行器在未知环境下的自主导航，但是SLAM方法计算量大不利于实时计算；文献[10～11]给出一种多状态约束的未知环境下视觉辅助惯性导航的自主导航方法；文献[12]给出一种利用未知陆标在连续2幅下降图像中的测量建立隐式测量模型对着陆器状态进行估计的方法，但该方法只适用于着陆面近似为平面的情况。上述方法虽然都避免了需要陆标位置的先验信息，但这些导航方法系统状态都不完全可观，因而探测器某些状态在着陆过程中并不能得到精确估计。

针对未知环境下行星表面导航问题，本文提出一种利用未知陆标序列图像测量的行星表面相对导航方法。该方法可有效估计探测器相对行星表面的速度，较为精确地估计探测器相对于着陆点或陆标的位置，从而可满足精确避障着陆的导航要求。以火星着陆为例的数学仿真表明该方法是有效可行的。

1 基于惯性测量的行星着陆运动学模型

以初始时刻星下点天南东系为着陆参考坐标系，假设惯性敏感器安装和零偏等偏差已经通过标定消除，探测器相对着陆坐标系的姿态可通过陀螺和星敏等敏感器精确确定，则利用加速度计测量建立探测器着陆运动学方程[12]

加速度计测量模型为

2 未知陆标图像隐式测量模型

隐式测量模型利用陆标序列图像测量信息结合探测器估计位姿构建隐式测量方程，该测量模型不需要陆标位置先验信息。

图1 未知陆标序列图像测量示意Fig.1 Schematic diagram of a unknown landmark observed in three sequential images

由式（4）可求解得到ti和tj时刻相机到陆标Pj的距离

根据式（3）和式（5）可计算ti+1时刻陆标Pj的位置为

则ti+2时刻陆标Pj在相机系下的视线矢量为

由式（7）和式（8）可看出，对任意位置未知的陆标，只要在3幅下降图像中观测到该陆标，即可建立该陆标的隐式测量方程用于探测器的状态估计。

3 基于未知陆标图像测量的导航滤波算法

3.1 状态方程

由（7）和式（8）可知，未知陆标的隐式测量模型同时与探测器在3个时刻的状态相关，故基于未知陆标图像隐式测量模型的自主导航系统状态向量取为

当相机拍摄一幅新图像后系统状态按如下方法确定

其中：xi+1和xi+2分别为成像时刻ti+1和ti+2之后的系统状态，M为

根据式（9）定义的探测器状态，可得探测器误差状态为

线性化式（1）得误差状态方程

3.2 隐式测量方程

由第2部分可得，ti+2时刻导航陆标Pj估计观测量为

则ti+2时刻观测量残差

根据状态误差定义，式（18）可写为

3.3 导航滤波器

采用EKF作为滤波算法对探测器状态进行估计。当惯性敏感器得到加速度测量数据时，系统状态可按式（1）进行递推，同时系统状态误差协方差可由下式积分得到

由于导航系统每获得一幅图像时均对系统状态按式（10）重置，故系统状态误差协方差P在每次成像时刻时按式（20）更新

假设有N个未知陆标同时在ti、ti+1和ti+2时刻拍摄的下降图像中被检测到，则可得观测残差向量

卡尔曼滤波增益计算为

导航系统状态和协方差更新为

4 仿真分析

为了验证本文提出的导航方案，以火星着陆为例进行仿真，参考“好奇号”（Curiosity）火星着陆探测器的轨道数据[13]以及降落伞和主发动机的减速能力，设置仿真初始条件：探测器在初始时刻星下点天南东系下位置和速度如表1所示。

表1 探测器仿真初始状态Table 1 Initial state parameters

假设探测器在着陆过程中始终保持相机指向着陆点方向。仿真初始条件为位置各方向存在1 000 m的随机误差，速度各方向存在10 m/s随机误差。设陆标方向矢量测量噪声为0.1＇，当没有陆标图像测量可用于导航更新时探测器状态根据惯性测量单元（Inertial Measurement Unit，IMU）测量积分得到。仿真采用模拟陆标进行，并在着陆过程拍摄的第一幅下降图像中选择安全着陆点，为了在下降图像中持续跟踪安全着陆点，需要在着陆点附近选取4个特征点且4个特征点中任意3个均不共线，从而根据射影几何交比不变定理利用选定的4个特征点在前后图像中的位置可实现对安全着陆点的连续跟踪，因此仿真中以选定的4个特征点及安全着陆点作为导航陆标进行仿真。需要注意的是，除着陆点外，每连续成像3次后根据相机视场重新选取4个特征点作为导航陆标，以保证可以在下降序列图像中存在可用的导航陆标。

图2和图3给出了探测器着陆轨迹的位置和速度变化，图4和图5给出了探测器着陆过程初始时刻星下点天南东系下位置速度的估计误差。从图4和图5中可看出，采用所提出的导航方法可以对探测器的速度进行精确估计，精度可优于0.1 m/s，但无法对位置信息进行修正，只能将位置误差维持在初始误差附近。值得注意的是，在未知环境着陆过程中，通常更关注探测器相对于着陆点的位置及速度信息。由于着陆点或障碍物在行星表面静止不动，因此探测器相对于着陆点的速度与着陆参考系下的速度相同，由图5可知其可被精确估计。探测器相对于着陆点的位置可通过估计的探测器位置以及着陆点在连续3幅下降图像中的视线矢量测量计算得到，如图6和图7所示，可见利用所提出的导航方法可有效估计探测相对于着陆点的位置，精度可优于1.5 m，满足精确避障着陆的要求。

图2 探测器位置Fig.2 The position in up-south-east frame during landing

图3 探测器速度Fig.3 The velocity in up-south-east frame during landing

图4 探测器位置估计误差Fig.4 The position estimation errors in up-south-east frame during landing

图5 探测器速度估计误差Fig.5 The velocity estimation errors in up-south-east frame during landing

图6 探测器相对于着陆点位置Fig.6 The relative position from the lander to the target landing site in upsouth-east frame during landing

图7 探测器相对于着陆点位置估计误差Fig.7 The relative position estimation errors from the lander to the target landing site.in up-south-east frame during landing

5 结 论

本文研究了一种利用未知陆标序列图像测量的行星表面相对导航方法。该方法首先利用未知陆标在连续3幅下降图像中测量得到的视线方向矢量结合探测器成像时刻位置建立了未知陆标的隐式测量模型，然后利用建立的未知陆标隐式测量模型通过卡尔曼滤波对探测器的位置、速度进行估计，最后利用估计的探测器位置结合着陆点的视线方向测量计算着陆点位置，进而获得探测器相对于着陆点的位置估计。仿真结果表明，提出的方法可有效估计探测器相对行星表面的速度，虽然无法直接估计探测器的位置，但可较为精确的估计出探测器相对于着陆点或陆标的位置，从而可满足精确避障着陆的导航要求。