基于IMU/FADS/无线电的火星上升器组合导航方案*

龚怡轩，刘 旭，马 瑞，朱庆华，李 爽

(1.南京航空航天大学 航天学院·南京·210016；2.中国电子科技集团第二十八研究所·南京·210007；3.上海航天控制技术研究所·上海·201109)

0 引 言

火星作为最靠近地球、与地球环境最为相似的行星，一直是人类行星探测的首选目标，火星探测任务也是近年来我国深空探测计划中的一个重要议题[1]。根据我国2016年发布的航天白皮书，我国将逐步开展火星无人着陆和采样返回任务。随着“天问1号”探测器选择于2020年7月择机发射，开展火星采样返回任务的深化论证和关键技术攻关将成为我国火星探测计划的下一个关键。在火星采样返回任务中，火星上升器(Mars Ascent Vehicle, MAV)将搭载样本从火星表面起飞，穿越火星大气后完成入轨对接，最终由轨道器将样本返回地球。MAV在经历了剧烈的火星进入、下降和着陆(Entry, Descent and Landing, EDL)过程和长时间的星表作业后，导航系统的标定参数将存在较大的不确定性。同时在上升过程中，火星复杂的大气环境也将给测量系统带来明显的干扰。此外，火星任务具有测控延时大的特点，因此有必要发展更加精确、自主的导航方案，以应对火星环境不确定性和上升器参数不确定性的影响。

目前，国际上针对火星采样返回任务上升段的导航方案还处于论证阶段，NASA和ESA均已提出过相应的火星采样返回方案，并开展了相关理论研究和试验验证工作。欧空局在2005年初步给出了一套火星上升导航、制导与控制(Guidance, Navigation and Control，GNC)方案，采用了IMU和星敏感器进行自主导航，入轨时刻半长轴的定位误差为8 km[2-3]。美国喷气推进实验室也开展了火星上升器的导航方案论证，计划采用IMU进行上升段自主航位递推[4]。马歇尔空间飞行中心的Evan等人进一步测试、对比了多个型号的IMU在火星上升段的导航精度，定位精度最高可达150m(均值)和65m(1-σ值)，测速精度最高为0.6(m/s)(均值)和0.3(m/s)(1-σ值)，但位置误差将在500s后发散到2000m量级[5]。南阿拉巴马州立大学的Carlos等人基于MATLAB/Simulink软件构建了火星上升器三自由度仿真系统，其导航子系统采用IMU和轨道器进行组合导航。蒙特卡洛仿真结果表明，入轨高度、速度、轨道半长轴和轨道倾角的最优结果分别为93m、0.022(m/s)、150m和0.01°(均为1-σ值)，已基本满足上升器入轨要求(定位均值优于100m，测速均值优于0.5(m/s))[6]，但文献给出的初始化误差较为保守。初始位置、速度和角速度误差为10m(均方根值)、0.1(m/s)(均方根值)和0.1(deg/s)(3-σ值)，对MAV的初始定位对准要求较高。

为进一步降低MAV上升段导航系统对初始定位对准的精度要求，同时满足火星上升入轨的导航精度，本文参考火星进入段组合导航方案进行火星上升段导航方案设计[7-13]。传统的导航方法基于惯性测量单元(Inertial Measurement Unit, IMU)进行航位递推，其精度有限。而火星大气对特高频无线电波的阻断效果不明显，所以无线电是理想的火星上升段导航敏感器。此外，嵌入式大气数据系统(Flush Air Data System, FADS)作为一种综合数据测量设备，可以提供气压高度、高度偏差、空速、马赫数、马赫数变化率、大气密度比等飞行参数和大气数据，已经在航空器和火星大气进入器中得到广泛应用[10]。例如，“火星科学实验室”采用基于IMU/FADS的测量方案实施进入段组合导航，探测器依靠IMU和FADS系统建立多个测量方程，然后通过三点法、五点法、最小二乘法等算法实现对大气参数和飞行状态的估计[11,13]。其中，利用IMU数据和FADS输出的动压信息估计飞行器速度是较为简单的一种方式，其本质是一种基于动压测量辅助的惯性导航方法，这也是本文所采用的方案。本文研究了基于IMU/FADS/无线电测量的上升段自主组合导航方法：利用火星轨道器和着陆器与上升器的通信链路进行无线电测距；利用Doppler测量获取上升器相对于轨道器和着陆点的惯性距离和速度；利用IMU测量上升器的加速度和角速度；利用大气传感系统测量动压和温度变化率等数据，从而获得上升器的相对速度；最后，通过非线性滤波方法融合多传感器信息，达到过滤噪声、提高导航精度的目的。

1 组合导航方案设计

基于IMU/FADS/无线电的组合导航方案如图1所示。探测器携带的导航设备包括了惯性导航系统、特高频(Ultra High Frequency，UHF)无线电收发装置，以及基于大气数据测量的FADS系统。其中，无线电导航方案选取了3个轨道器作为无线电导航信标。测压系统则参考了好奇号的分布方式，由十字形分布的7个测压孔构成。测压孔可以得到探测器周围的动压、温度等数据，将多个测压孔的测量信息进行联立，利用特殊的三点组合进行解耦，即可得到探测器的动压、温度、速度方向等数据。

图1 基于三轨道器的IMU/FADS/无线电导航组合方案

1.1 IMU测量模型

IMU测量装置包含三轴正交的加速度计和陀螺仪，分别用来测量本体系下的火星上升器的加速度和三轴姿态角速度，测量模型可以写为：

(1)

其中，aB和ωB分别为真实的加速度和角速度；Γa表征由加速度计轴的正交误差和未对准误差所造成的影响；Γω则表示由陀螺仪的非正交性误差和未对准误差所造成的影响；ba和bω分别为加速度计常值漂移和陀螺仪常值漂移，设为常值误差；Sω和Sa分别为由陀螺仪刻度误差和加速度计刻度误差所造成的影响；ξa和ξω分别为加速度计和陀螺仪的测量噪声，均服从0均值高斯分布。若忽略Γa、Γω、Sa、Sω等高斯型小量，IMU的测量模型可以写为：

(2)

1.2 FADS测量模型

(3)

(4)

(5)

动压的单位矢量为qe=-ve。ve表示本体坐标系下上升器相对速度的单位矢量。在考虑动压、温度变化率误差后，可以将FADS系统的测量模型写为：

(6)

1.3 无线电测量模型

轨道器发射无线电，而着陆器通过对接收到的无线电信号进行分析，可由以下表达式来计算两者之间的相对距离[11,14]：

纳入标准:(1)能生活自理的0级糖尿病足患者。(2)能顺利沟通,知情且同意参与。排除标准:(1)糖尿病酮症,急性感染,足部感觉异常。(2)下肢血管手术后及功能障碍。

Ri=|ri-r|

(7)

(8)

(9)

其中，r为着陆器在火星惯性系下的位置矢量；ξRi为对应无线电信标距离测量噪声；ri为第i个轨道器在火星惯性系下的位置矢量；μm为火星引力常数；O(J2J3J4)为火星非球形摄动项；O(S)为太阳引力摄动。而将着陆器速度记为v，第i个轨道器在火星惯性系下的速度矢量记为vi，ζVi是速度测量噪声，则轨道器与着陆器的相对速度可以写成如下形式：

(10)

Vi=|vi-v|

(11)

无线电测量模型则可写成：

(12)

其中，R为由多个无线电轨道器相对距离构成的列向量，V、ξR、ξV同理。

1.4 导航测量方程

根据以上敏感器的测量模型，可以将导航测量方程写为如下形式：

(13)

2 组合导航算法

2.1 系统状态方程

状态变量选取上升器在火星惯性系中的位置r、速度v和三轴姿态角Ω，则状态量可定义为X=[rT,vT,ΩT]T，可以将系统状态方程写为：

=f(x(t),t)+w

(14)

2.2 UKF算法

下面给出UKF滤波算法流程，导航系统的状态方程和测量方程为[15]：

(15)

(1)滤波初始化：

(16)

(17)

(2)通过UT变换，得到sigma及其权值：

(18)

其中，n为变量维数，参数λ=a2(n+κ)-n，a的选择控制了采样点的分布，通常取一个较小数(如0.001)，待选参数κ取为0。下标m表示均值，c表示协方差，权系数β取为2。

(3)进一步预报状态和协方差矩阵：

(19)

(20)

(4)计算预测测量值及其协方差：

(21)

(22)

(23)

(24)

(5)计算滤波增益，并更新状态矩阵和协方差矩阵：

(25)

(26)

(27)

2.3 AUKF算法

能否获取准确的先验信息对滤波算法的性能表现有着很大的影响。而在火星上升段导航中，由于环境不确定性和复杂动力学环境等因素的干扰，自主导航对滤波算法的稳定性提出了更高的要求。目前广泛使用的卡尔曼滤波(Kalman Filter，KF)、UKF、扩展卡尔曼滤波(Extended Kalman Filter，EKF)等算法[16]，其过程噪声协方差矩阵Q和测量噪声协方差矩阵R通常是在仿真开始前选取好的，在接下来的滤波迭代过程中保持不变。而在实际导航过程中，非线性系统的系统噪声、敏感器的测量噪声通常都是未知且实时变化的，矩阵Q和矩阵R的准确程度将会影响估计的准确性和稳定性[17]。本节采用的自适应无迹卡尔曼滤波算法(Adaptive Unscented Kalman Filter, AUKF)将针对以上情况，对系统噪声协方差矩阵Q和测量噪声协方差矩阵R进行调节，使矩阵Q和矩阵R跟随状态估计值和误差协方差矩阵一起实时更新，从而提高导航的精度和稳定性。在UKF基础上，AUKF增加了如下的更新过程：

(28)

Rk=(1-dk-1)Rk-1+dk-1(εkεkT-Pz)

(29)

dk=(1-b)(1-bk)-1

(30)

εk=yk-zk

(31)

该过程对矩阵Q和矩阵R进行了更新。其中，dk为缩放参数；b为遗忘因子，通常的选取范围为0.9～0.995，本文设置为0.995；残差项εk=yk-zk。

3 仿真分析

为验证本文提出的组合导航方案的有效性，通过仿真对比验证IMU/FADS组合导航方案和IMU/FADS/Radio组合导航方案，仿真均在MATLAB/Simulink环境下进行。“火星科学实验室”探测器FADS系统的响应频率为8 Hz，文献[6]中IMU的响应频率为200 Hz，而“黑障”效应对特高频无线电的阻隔效果不明显，无线电全程可测。因此，为简化仿真，IMU、无线电、FADS的响应频率可设置为10Hz，仿真时长设置为250s，仿真步长为0.1s。上升器的初始三轴位置误差的均方根值为100m，初始三轴速度误差的均方根值为1(m/s)，初始三轴姿态角误差的3-σ值为0.1°，上升器的标准初始位置如表1所示，表2则给出了作为无线电信标的轨道器的初始状态和噪声设置。

表1 上升器的初始状态

表2 轨道器的初始状态和噪声设置

上升器的初始状态误差协方差矩阵、系统过程协方差矩阵可分别设置为：

P0=diag([104,104,104,1,1,1,10-4,10-4,10-4])

(32)

Q=diag([100,100,100,0.1,0.1,

0.1,10-10,10-10,10-10])

(33)

3.1 IMU/FADS组合导航

IMU/FADS组合导航方案采用IMU和一个安装在上升器头部的FADS系统作为导航敏感器，其仿真结果如图2所示。图2(a)～图2(c)分别表示了MAV的三轴位置误差、速度误差及姿态角误差，合位置误差及合速度误差如图2(d)和图2(e)所示。

(a)三轴位置估计误差

通过IMU/FADS组合导航的仿真结果可知，FADS测量信息的加入可以有效控制航位递推中位置与速度估计的发散趋势，而在纯IMU航位递推情况下，位置误差最终可达10km，速度误差可达70(m/s)。因此，IMU/FADS组合导航精度与传统的航位递推相比有了很大提高。但是，IMU/FADS组合导航的收敛情况稍差。在仿真结束时，MAV的合位置误差的均方根为900.7m，合速度误差的均方根为37.6(m/s)，滚转角、俯仰角、偏航角的均方根值分别为0.01504°、0.00838°和0.03521°，不能满足上升段对导航精度的要求。

由图2(a)和图2(b)可知，虽然IMU/FADS组合导航有效控制了测量发散，但其收敛性较差，比如其X轴、Y轴的位置估计和Y轴的速度估计仍然存在不能收敛的现象。位置估计不能收敛是因为FADS系统不能直接测量上升器的位置与高度，而只能通过速度积分来对位置信息进行补充，这种方法对位置误差的修正效果有限。Y轴速度误差不能收敛是因为MAV在上升过程中沿Y轴的速度分量变化不大，使FADS难以获得Y轴速度分量的有效信息，最后导致发散。需要说明的是，IMU/FADS导航方案本质上是一种基于动压测量辅助的惯性导航方法，其主要依赖高精度IMU递推，因此最终的导航精度受IMU本身精度影响较大。

3.2 IMU/FADS/Radio组合导航

相比于IMU/FADS组合导航方案，该方案增加了3个轨道器信标作为外部导航敏感器，IMU/FADS/无线电组合导航的仿真结果如图3所示。图3(a)～图3(c)分别表示了上升器的三轴位置误差、速度误差及姿态角误差，合位置误差以及合速度误差如图3(d)和图3(e)所示。

(a)三轴位置估计误差

由图3(d)和图3(e)可知，IMU/FADS/无线电组合导航的仿真结果相比之前的IMU/FADS方案有了明显的提高，多信息融合使导航的可观性大大提高，也使估计结果更加精准。表3给出了不同导航方案状态估计误差结果对比，其中合位置误差的均方根值为34.1 m，合速度误差的均方根值为0.3797(m/s)，三轴姿态误差均方根值分别为0.00112°、0.00084°和0.00149°，满足火星上升段导航需求。

表3 不同导航方案状态估计误差结果对比

同时，图4和图5给出了IMU/FADS/无线电组合导航方案下AUKF和UKF的对比情况。可以看到，由于AUKF的过程噪声矩阵具有自适应性，可以随着状态值、误差协方差矩阵的更新而更新，所以其相邻数据差更小，AUKF的误差曲线振荡幅值更小，系统稳定性更优。当UKF出现较大波动时，AUKF可以很好地保持结果的稳定性。但是，如表4所示，使用AUKF进行估计时的合位置误差均方根值、合速度误差均方根值分别为34.2982 m和0.3732(m/s)，与之对应UKF的合位置误差均方根值、合速度误差均方根值分别为34.1440 m和0.3796(m/s)。可以看到，对比分析两种滤波在仿真结束时的均方根值，差别并不大。因此，在目前的IMU/FADS/无线电导航方案下，AUKF可以有效提高状态估计的稳定性，其整体相邻数据差远小于UKF，但其对估计精度的提高有限。这是由于本文中UKF预设系统过程噪声协方差矩阵比较贴近真实的系统噪声值，以及在本方案中系统噪声的实时变化值并不大，导致AUKF自适应的优势没有得到很好的体现。在后续研究中，需将选择系统过程噪声设置为较为极端的情况，以测试AUKF的性能。

图4 AUKF与UKF合位置误差对比

图5 AUKF与UKF合速度误差对比

表4 UKF与AUKF估计误差对比

4 结 论

本文针对火星采样返回任务中火星表面上升段自主导航问题，首先通过融合惯导测量信息、无线电测距测速信息及FADS系统测量的大气数据信息，发展出基于IMU/FADS/无线电测量的火星上升段高精度组合导航方法。该方法利用了轨道器资源和在航空领域被广泛应用的大气数据传感系统，对IMU测量信息进行了补充，并采用UKF算法进行了导航信息融合。仿真结果表明，采用IMU/FADS/无线电测量方案的火星上升段组合导航方案相比IMU/FADS方案具有更高的导航精度和稳定性。同时，还对比测试了AUKF和UKF算法的稳定性。由仿真结果可知，相比于UKF算法，AUKF的估计稳定性更优，可有效提高MAV导航系统的抗干扰能力。