基于自适应均方根EKF的惯性辅助航天器仅测角导航方法

杜荣华，廖文和，张 翔

（南京理工大学 机械工程学院，南京 210094）

随着航天技术的持续发展，航天器任务范围也从传统的通信、导航和遥感等应用领域向在轨服务[1]、空间碎片清理[2]、编队飞行[3]和深空探测[4]等新型航天器任务拓展。这些新型航天器任务都涉及了针对空间非合作目标的交会操作，相关技术也成为了国际竞争前沿科技。

为了成功实施针对空间非合作目标的交会操作，亟需提升航天器的自主运行能力。这其中涉及空间非合作目标的自主检测、自主相对导航和自主制导与控制等方面的研究内容。本文重点研究了其中的自主相对导航问题，即航天器能够通过星载相对导航传感器，自主获取空间非合作目标相对于航天器的相对状态信息。目前，常用来测量空间物体之间相对状态信息的有源主动式传感器（例如激光和微波雷达等），由于它们功耗、质量和体积都比较大，使得它们无法应用在微小卫星平台上，而无源被动式传感器（例如可见光和红外相机等）在这一方面具有较大的优势[5,6]，因为它们对航天器的总质量和功耗的设计影响很小[7]，且适用于各种航天器间距。此外，许多航天器配备了星敏感器，如果方向合适，可用于拍摄其视野范围内的空间目标，并根据测得的视线矢量信息执行相对导航任务[8]。其中，仅测角导航技术代表了多种高级分布式空间系统的明确使能技术，包括空间态势感知、自主交会对接、分布式孔径科学和涉及空间非合作目标的在轨维修等任务[9]。

仅测角导航技术指的是通过单个相机在一段时间内测量航天器和空间目标之间的视线矢量，以推导出它们之间的相对状态信息。因为单目相机深度信息的缺失，仅测角导航存在星间距离不可观测的问题[10-12]。目前，已经有许多学者研究了提高仅测角导航系统可观性的方法，例如轨道机动法[13]、相机偏置法[14]、非线性动力学法[15]和多星多传感器法[16]。由于视觉传感器存在视场角小和数据更新率低等缺点，而将航天器上既有的惯性测量单元（Inertial Measurement Unit,IMU）引入相对导航系统能够克服单一测量系统存在的缺陷，具有功耗小、输出频率高、延迟低和精度高等优势。目前，已经有部分学者开展了视觉和惯性组合相对导航方法的研究。Zhang[17]等针对空间交会对接地面混合悬浮微重力模拟实验中自主水下航行器的高精度实时导航问题，提出了一种复杂多介质环境下视觉/惯性自适应组合导航方案和算法。Cai[18]分析了CCD相机所拍摄的序列影像中的特征点观测量，与惯性导航系统进行组合，提出了基于多状态约束的视觉/惯性组合导航算法，实现了在无GNSS环境下利用低成本惯性器件的导航定位。

针对航天器对空间非合作目标交会操作中的自主相对导航问题，本文提出了一种基于自适应均方根EKF的惯性辅助航天器仅测角导航方法。通过将惯性测量数据引入仅测角导航系统，不仅克服了视觉传感器输出频率低、视场角小和易受外界干扰的缺陷，还可以利用视觉传感器长周期、高精度的测量性能来修正IMU的漂移累积误差。首先，推导了一种基于CW方程[19]的仅测角导航模型。然后，基于误差状态推导了一种仅测角/惯性组合相对导航模型。接着，基于Cholesky分解得到组合导航系统的状态协方差矩阵，并引入“新息”协方差匹配方法对测量噪声进行在线自适应调整，构建了一种自适应均方根EKF，对该组合导航系统状态进行动态滤波估计，并采用Fisher信息矩阵对该组合导航系统的可观性进行了评估。最后，在搭建的半物理仿真平台上对提出的基于自适应均方根EKF的惯性辅助航天器仅测角导航方法进行了仿真分析与验证，证明了该方法的有效性。

1 仅测角导航模型

1.1 相对动力学模型

采用著名的CW方程作为主星和副星之间自主交会的相对动力学模型。该CW方程建立在笛卡尔坐标系“car”中，如图1所示，其中x轴背离地心，z轴垂直主星的轨道平面，y轴在轨道平面内且与x轴和z轴满足右手定则。若不考虑摄动，则CW方程为：

图1 笛卡尔坐标系“car”的定义Fig.1 Definition of Cartesian coordinate system “car”

其中，ω为主星的轨道角速率，x、y和z为相对位置分量，ux、uy和uz为副星的推力加速度分量。

假设副星没有进行轨道机动操纵，即ux、uy和uz均为零，则对方程(1)进行积分之后得到CW方程的状态转移矩阵为：

1.2 视线测量模型

在定义视线（Line-of-Sight, LoS）矢量之前，首先需要先定义相机坐标系“cam”。为简单起见，在不失一般性的情况下，假设相机视轴与航天器反飞行方向对准。在这个假设下，相机坐标系“cam”下的相对位置矢量rcam和笛卡尔坐标系“car”下的相对位置矢量r之间的关系为：

LoS矢量包括一组方位角α和俯仰角ε，可以表示为相机坐标系“cam”下的相对位置矢量rcam的函数，其关系如图2所示。因此，LoS测量模型可表示为：

图2 LoS测量几何Fig.2 LoS measurement geometry

为了计算LoS测量值相对于状态量x的测量灵敏度矩阵H，首先需要计算测量值相对于相机坐标系“cam”下的相对位置矢量rcam的测量灵敏度矩阵，并且有：

通过方程(8)将方程(10)得到的偏导数映射到笛卡尔坐标系“car”下，进而得到测量灵敏度矩阵H为：

2 组合相对导航模型

在松耦合模式下，惯性辅助航天器仅测角导航系统的组合相对导航模型如图3所示。在仅测角导航系统正常工作的情况下，将惯性导航系统与仅测角导航系统的相对位置和速度之差作为量测信息输入到卡尔曼滤波器中，然后通过滤波估计校正IMU的漂移累积误差，最终得到组合导航系统的相对状态信息。

2.1 误差状态模型

惯性辅助航天器仅测角导航系统的误差状态量包括相对位置误差、相对速度误差、副星平台失准角误差、加速度计零偏和陀螺仪常值漂移等，后面将对各个误差状态量做详细介绍。

如图1所示，选取以主星质心为原点的局部轨道坐标系作为相对导航参考坐标系。在相对导航参考坐标系中，相对位置和速度的误差状态方程为：

其中，Δr˙为相对位置误差的一阶导数；Δv˙为相对速度误差的一阶导数；C41～C63为重力加速度相对于位置矢量的导数，此处假定都为零。

假定IMU采用正装方式，并且三轴方向与副星本体坐标系对齐，以及忽略加速度计测量误差中的二阶及二阶以上量，则加速度计的测量误差状态方程为：

式(13)的展开形式为：

其中，a表示加速度计测量的加速度值；Θ表示副星平台失准角；表示加速度计的零偏；表示加速度计测量值的高斯白噪声；表示从副星本体坐标系至相对导航参考坐标系的转换矩阵。

忽略陀螺仪误差的二阶及二阶以上量，副星平台失准角误差方程可表示为：

式(15)的展开形式为：

综合上面几个误差状态方程，则惯性辅助航天器仅测角导航系统的误差状态方程为：

其中，误差状态变量δ x为：

式(17)中的F表示的是与非线性动力学相关的雅克比矩阵，G和w表示的是与系统噪声相关的矩阵，各符号的具体表达式为：

误差状态方程(17)的离散形式为：

其中，Φk|k-1为状态转移距阵，可表示为：

其中，Δt为滤波周期。

wk为系统噪声，满足高斯白噪声过程，即：

其中，Qk为系统噪声协方差矩阵，Qk具体表达式为：

2.2 测量模型

在松耦合模式下，组合相对导航系统以惯性/仅测角导航系统估计的差值作为组合导航滤波器的测量输入值，则测量方程具体可表示为：

其中，

其中，下标“Imu”表示惯性导航系统参数；下标“Ang”表示仅测角导航系统参数；vk为测量噪声，满足高斯白噪声过程，即：

其中，Rk为测量噪声协方差矩阵；并且vk和wk不相关，即：

3 自适应均方根EKF

EKF和均方根EKF的流程图如图4所示，包括状态传播和测量更新两个主要步骤。其中，更新方程是标准EKF和平方根EKF的主要区别。当协方差矩阵条件不佳时，标准EKF在更新步骤中可能会使协方差矩阵失去正定性。平方根EKF的更新方程比标准EKF的更新方程要复杂得多，但是它们可以提高数值精度并保持协方差矩阵的正定性。本文使用的平方根算法采用了Maybeck提出的方法[20]。系统状态协方差矩阵的平方根可以通过Cholesky分解计算得到：

图4 EKF和平方根EKF的流程图Fig.4 Flowchart of EKF and square-root EKF

由于过程噪声和测量噪声的选择对卡尔曼滤波器的性能至关重要，适当地选择过程噪声和测量噪声可以提高卡尔曼滤波器的动态滤波性能。因此，本文引入“新息”协方差匹配方法对测量噪声进行在线自适应调整。“新息”协方差匹配的基本思想是利用样本的统计数据及其理论值来推断滤波器的测量噪声，并对其进行自适应调整。在正常滤波情况下，滤波器的测量预测误差（也称“新息”或“残差”）为：

其中，vk为零均值高斯白噪声，它表示组合导航系统的测量噪声。

将式(33)带入式(32)，“新息”可以重新表示为：

通过应用期望得到理论上Δk的协方差ΣΔk为：

其中，Rk为滤波测量噪声协方差矩阵；k|k-1为相对于先验状态估计|k-1的协方差矩阵。

另一方面，如果在滑动窗口中收集N个样本，则样本的采样协方差ΣˆΔk可表示为：

根据式(35)和式(36)的等效关系来计算测量噪声协方差矩阵的估计值为：

滤波测量噪声协方差矩阵Rk与测量噪声协方差矩阵的估计值之间的差值为：

4 基于Fisher信息矩阵的可观性分析

为了评估惯性辅助航天器仅测角导航系统的可观性大小，有必要建立仅测角导航可观性的定量分析手段。Fisher信息矩阵可用于测量给定测量集所获得的信息量大小。首先，对于一个离散非线性估计系统可表示为：

其中，xk是状态向量，wk是过程噪声，zk是测量输出，vk是第k个时间步长的传感器测量噪声。

令xˆk|k是状态向量xk的无偏估计量，则滤波估计问题的误差协方差矩阵可被定义为：

式(41)中的误差协方差矩阵的下界可表示如下：

其中，Jk称为Fisher信息矩阵，其逆矩阵称为Cramér-Rao下界（Cramér-Rao Lower Bound, CRLB），CRLB定义了组合导航滤波器估计性能的下限。Bar-Shalom[21]指出，如果滤波器的方差等于CRLB，则滤波器可以认定为有效。在本研究中，假设实施了有效的估算，则可以直接采用CRLB来评估组合导航系统的可观性大小。

Fisher信息矩阵可以采用递归形式计算得到：

其中，Φk+1|k是状态向量从时间步长k到k+1的状态转移矩阵，Hk是测量方程相对于估计状态的雅可比矩阵，Rk表示在第k个时间步长的传感器测量噪声矩阵。用初始协方差矩阵P0的逆矩阵初始化此递归方程，得到：

由于Fisher信息矩阵是矩阵形式的，因此以前的大多数研究都通过该矩阵建立的标量来建立优化问题的成本函数。其中，最常用的方法是在终端时刻处取Fisher信息矩阵的行列式，即

在式(45)中，负号用于建立最小化问题。由于对称正定矩阵的行列式等于其特征值的乘积，因此它直接与估计不确定性椭球的体积有关。最终，基于Fisher信息矩阵的可观度分析方法可以获得终端时刻组合导航系统的可观性大小。

5 仿真校验

5.1 半物理仿真测试平台搭建

在完成了基于自适应均方根EKF的惯性辅助航天器仅测角导航算法的设计后，搭建验证该算法的半物理仿真平台，其结构框图如图5所示，实物图见图6。该半物理仿真平台配备了三轴直线导轨和三轴精密转台等机械微调装置，可以调整星图显示装置和星图采集模块同轴。

图5 半物理仿真平台结构框图Fig.5 Block diagram of the semi-physical simulation platform

图6 半物理仿真平台Fig.6 Semi-physical simulation platform

该半物理仿真平台主要包括四大模块：

(1) 姿态/轨道发生器：该模块内部装有Matlab＆simulink软件以及STK软件，主要用于生成副星和主星的姿态/轨道数据，模拟出副星和主星的运动轨迹参数；

(2) 星光星图模拟模块：该模块作为星光星图模拟器，内部装有恒星星库、星图生成软件等，主要用来模拟恒星和主星在星图中的分布情况和星光等级；

(3) 星图识别与目标检测模块：该模块内部装有星图识别软件、星体质心定位软件和空间非合作目标检测软件等，主要功能是在原始图像中找出主星，并输出主星相对于星图采集模块的LoS测量信息；

(4) 仅测角导航模块：该模块的主要功能是对本文提出的惯性辅助航天器仅测角导航算法进行实时运算，并输出主星和副星之间的相对状态信息。

上述四大模块中，姿态/轨道发生器和星光星图模拟模块为同一台计算机，选用型号为Lenovo，Windows 7，Intel Core i7系列处理器；星图识别与目标检测模块和仅测角导航模块为同一台计算机，选用型号为Lenovo，Windows 7，Intel Core i7系列处理器。此外，星图显示装置选用型号为宝莱纳公司的4:3正屏工业显示屏B170，其分辨率为1280×1024；星图采集模块选用型号为EV76C660ABT-EQTR的CMOS图像传感器，分辨率为1280×1024，像素大小为5.3 μm×5.3 μm，焦距为25 mm的相机。另外，可以将星图显示装置放置于准直透镜的焦平面处，且靠近星图采集模块的镜头处。经过准直透镜的星图就可以模拟无穷远处的恒星和主星在图像中的分布情况，力求模拟出的星图成像效果能更加接近真实的场景。

5.2 试验测试与结果分析

该惯性辅助航天器仅测角导航算法的半物理仿真输入条件如下：

(1) 相对位置误差为ROE1：迹向-2000 m、径向和法向200 m @ 30 km；ROE2：迹向-1200 m、径向和法向120 m @ 20 km；ROE3：迹向-300 m、径向和法向30 m @ 5 km。这三种轨道场景中的主星和副星的初始绝对轨道根数见表1，包括轨道半长轴a、轨道偏心率e、轨道倾角i、升交点赤经Ω、近地点幅角ω和平近点角M。仿真校验主要在三种典型的航天器交会场景中进行，包括：ROE1代表主星开始接近副星的可能配置，表示远距离保持点相对运动轨迹；ROE2代表每个轨道周期主星向副星漂移近1公里，表示自然漂移相对运动轨迹；ROE3代表对接阶段的起点，表示主副星随动相对运动轨迹。这三个航天器交会场景刚好包含了主星从远距离交会抵近至副星的各个过程；

表1 轨道参数Tab.1 Orbital parameters

(2) 相对速度误差：0.1 m/s、0.01 m/s、0.01 m/s；

(3) 平台失准角误差：0.3 °、0.3 °、0.3 °；

(4) 陀螺仪测量误差：常值漂移为0.035 °/h，随机漂移方差为σ2= (0.035 °/3 h)2；

(5) 加速度计测量误差：常值零偏为0.00001g，随机零偏方差为σ2= (0.00001g/3)2；

(6) 仅测角导航系统采样周期：TAng= 10 s；

(7) 惯性导航系统采样周期：TImu= 1/30 s；

(8) 组合导航系统滤波周期：Tfilter= 1 s；

(9) 组合导航系统总滤波时间：Ttotal= 3000 s。

首先，在这三种轨道场景下，对惯性辅助航天器仅测角导航系统的可观性D值进行了仿真分析，结果见表2。从表2中可以看出，三种轨道场景中的仅测角导航系统的可观性D值相差不大，其中轨道场景ROE2的可观性最好，其次是轨道场景ROE1和轨道场景ROE3。

表2 仅测角导航可观性D值仿真结果Tab.2 Angles-only navigation observability D-value simulation results

此外，在搭建的半物理仿真平台上，对提出的基于自适应均方根EKF的惯性辅助航天器仅测角导航算法进行了仿真分析，结果如图7-9所示，为三种轨道场景下的惯性辅助航天器仅测角导航滤波器的误差曲线图。从图7-9中可以看出，本文提出的惯性辅助航天器仅测角导航滤波器能够成功收敛，且本文提出的自适应均方根EKF（图中显示为SREKF）的收敛速率比EKF要快，基本上在400秒以内就实现了收敛。此外，终端时刻采用自适应均方根EKF比EKF的迹向滤波精度平均提高了35%，径向和法向滤波精度平均提高了25%。因此，证明了本文提出的基于自适应均方根EKF的惯性辅助航天器仅测角导航算法的有效性。

图7 在ROE1中，惯性辅助航天器仅测角导航算法的误差曲线Fig.7 Error curves of the inertial-aided spacecraft angles-only navigation algorithm in ROE1

图8 在ROE2中，惯性辅助航天器仅测角导航算法的误差曲线Fig.8 Error curves of the inertial-aided spacecraft angles-only navigation algorithm in ROE2

图9 在ROE3中，惯性辅助航天器仅测角导航算法的误差曲线Fig.9 Error curves of the inertial-aided spacecraft angles-only navigation algorithm in ROE3

6 结 论

本文提出了一种基于自适应均方根EKF的惯性辅助航天器仅测角导航方法。通过将航天器上配备的IMU的测量数据引入仅测角导航系统，不仅克服了视觉传感器输出频率低、视场角小和易受外界干扰的缺陷，而且可以利用视觉传感器长周期、高精度的测量性能来修正IMU的漂移累积误差。最后，通过搭建的半物理仿真平台验证了本文所提出的基于自适应均方根EKF的惯性辅助航天器仅测角导航方法，得出所提出的自适应均方根EKF的收敛速率比EKF要快，基本上在400 s内就实现了收敛。并且，终端时刻采用自适应均方根EKF比EKF的迹向滤波精度平均提高了35%，径向和法向滤波精度平均提高了25%。