柱面坐标系下航天器仅测角相对导航算法

龚柏春，张德港，张伟夫，苑艳华，陈修桥

（1. 南京航空航天大学，南京210016；2. 上海卫星工程研究所，上海 201109；

3. 北京控制与电子技术研究所，北京100038；4. 中国人民解放军32032部队，北京100094）

当前，地球同步轨道的环境越来越复杂，大量增加的失效卫星、故障卫星、碎片以及太空武器等（统称为空间非合作目标）给在轨现役卫星带来各种主被动安全威胁。为了应对空间非合作目标的安全威胁，各国都在大力发展包括空间态势感知、在轨服务等在内的空间安全技术，而对目标进行及时、长时、精确的相对导航（或称相对轨道确定）则是关键前提技术。

通常，能用于对空间非合作目标进行相对测量的敏感器包括微波雷达、激光雷达、光学相机等。其中，微波雷达、激光雷达等系统因为系统复杂、造价昂贵、能耗大等缺点难以在快速响应的中小型卫星上配备。而光学相机因为其具有简单可靠、体积小、重量轻、功耗低、全自主等特点已经被在轨卫星广泛应用。同时，光学相机的无源测量也具有很好的隐蔽性，相比雷达更加适合在空间攻防领域的应用。然而，也正是因为光学相机的无源测量，使得其只能获取目标的视线角信息，缺少测距信息，这就产生了仅测角相对导航的状态不可观测/弱可观测的问题[1]。国内外的学者从多个角度对该问题进行了研究。

Wang[2]和Chen[3]等人提出了一种双视线测量的仅测角相对导航实现方法，Han等[4]对此方法提出了显著提高距离状态可观测度和估计性能的优化方法,该方法通过配置辅助测量航天器形成测量基线，从而来引入距离信息，双星编队获取测量基线“边”，然后与双星同时测量的视线角一起以“角边角”的方式确定三角形的形状，也就解决了相对距离在可观测性方面的问题，但是这种方法需要至少2颗卫星，增加了成本支出。Anjaly[4]提出了利用轨道机动信息进行距离估计的思想，研究了可观测性最优的机动方式，但是轨道机动法约束了实际操作任务中相对轨道制导的自由度，同时也带来了更多的燃料消耗与安全风险，Gao等[6]验证了轨道机动对可观测性的影响，Klein[7]提出了一种利用测量相机安装存在偏离航天器质心的现象，解决距离可观测性的新思路和新方法——相机偏置法，在相机偏置法的基础上，Gong等[8]建立了基于无迹卡尔曼滤波的仅测角相对导航算法，Du等[9]提出了快速获得仅测角相对定轨解的方法。相机偏心距离足够大的时候就可以提供距离的可观测性，但是相机偏置法有效作用范围取决于相机偏离航天器质心的距离，通常这个距离比较小，因此只适用于近距离探测的情况。Kaufman等[10]从笛卡尔坐标系下二阶非线性相对运动动力学出发，通过采用高阶李导数研究了仅测角相对轨道确定的非线性可观测性问题。Li等[11]说明了通过非线性模型可以解决原本仅测角相对导航在笛卡尔坐标系下不可观测的问题。Gaias等[12]在相对轨道要素模型下探讨了仅测角相对导航问题，分离了不可观测的轨道要素。Han等[12]建立了球坐标下的状态方程和观测方程，采用了UKF算法，推导了滤波参数的计算方法。然而，非线性动力学模型的非线性强弱会直接决定可观性的强弱，如果非线性太弱，则产生的效果很容易淹没在测量误差之中。

综上，现有方法主要分成多敏感器协同测量、轨道机动辅助、敏感器杆臂效应辅助以及非线性动力学等四类。多敏感器法需要至少两颗卫星协同测量，且对构型有一定要求，增加了经济成本；轨道机动辅助法增加了燃料消耗，也带来了碰撞风险；敏感器杆臂效应法限于杆臂长度仅能适用于公里级的近程场景；非线性动力学法从模型的角度提供可观测性，牺牲一定的计算量来实现仅测角导航，但这对于目前高速发展的计算能力来说已经不是问题。

因此，本文从利用非线性动力学解决仅测角导航可观测性的角度出发，研究在轨道曲率捕获能力更强的曲线坐标系下建立相对运动动力学模型并用于仅测角相对导航。下面将先建立柱面坐标系下的航天器相对运动动力学模型以及视线角测量模型，然后进行相对轨道状态的可观测性分析，接着引入平方根容积卡尔曼滤波建立非线性滤波算法、并设计长航时导航方案，最后对所提出的算法进行数值仿真验证。

1 相对运动动力学模型

美国学者Geller等[14]在柱面坐标系下建立了二维平面情况下的航天器相对动力学模型，本文在此基础上进行拓展，在柱面坐标系下建立三维空间的相对运动动力学模型。如图1和图2所示，首先，建立一个与主星（Chief）初始轨道平面重合的固定参考轨道平面，这个平面的法线将用单位矢量iz表示，在参考平面上的分量用2个极坐标的单位矢量iρc、iθc表示。因此，三维空间的位置矢量可由iρc、iz和iθc线性组合表示。

图1 柱面坐标系下的X-Y平面示意图Fig.1 Schematic diagram of 2D in cylindrical coordinate

图2 柱面坐标系下的三维示意图Fig.2 Schematic diagram of 3D in cylindrical coordinate

以主星为例，将地心与卫星的连线用矢量表示，并求1阶和2阶导数后可得：

将这个运动方程与二体动力学方程在柱面坐标系下的表示形式做比对即可得到柱面坐标系下的轨道动力学方程：

类似的，可以得到从星（deputy）在柱面坐标系下的轨道动力学方程。

定义相对运动状态为：

那么相对状态的二阶导数为：

令柱面坐标系下的相对轨道运动状态量为：

则相对运动状态模型可以写成如下的非线性形式。

后续的导航滤波估计中将采用式(8)进行相对轨道状态的演化。

2 量测方程

设相机测量得到的视线角为α和β，根据图1、图2所示的几何关系可以得到测量角与相对状态的关系式如下：

最终的测量方程建模为：

其中，α和β相对角度的真实值，vθ、wθ是对应的测量噪声，通常假设为零均值高斯白噪声，d是主星和从星在主星轨道平面上投影的距离，可由其他参数计算得到，即。

对于主星轨道有倾角的情况，为了方便计算可按图3所示关系对惯性系与近焦点系进行坐标转化，从而与柱面坐标系建立联系。其中，近焦点坐标系是以主星轨道所在平面为XY平面建立近焦点坐标系，该坐标系的坐标原点位于地心，Z轴与轨道角动量方向重合，X轴指向近地点，Y轴与XZ平面垂直构成右手系；地心惯性坐标系是X轴指向春分点，Z轴与地球旋转轴重合，向北为正，Y轴与XZ平面垂直构成右手系。

图3 惯性系与近焦点系的关系示意图Fig.3 Relation of the inertial system and the near focal system

3 可观测性分析

通常，对非线性系统进行可观测性分析可采用李导数的方式进行。但是如式(8)(10)所示的柱面坐标系下相对导航模型是包含三角函数在内的强非线性模型，利用高阶李导数证明可观测性分析时会包含大量复杂的三角函数计算，很难在不进行大量线性化的情况下获得解析解，而线性化又会略去用来提供仅测角可观测性的非线性项。因此，下面只对柱面系下的仅测角导航模型的可观测性进行定性分析。

文献[1]中总结了仅测角相对轨道不可观测的四个原因，其中之一便是线性化动力学模型假设，也就是说采用线性化动力学模型进行轨道演化时仅测角相对导航是不可观测的。而本文中柱面系下的仅测角导航模型是强非线性的，那么理论上可观测性也越强。接下来分析模型对轨道曲率的捕获能力。测量角有α和β，α和β的测量方程如式(10)所示，将α和β分别对ρrel、θrel、zrel求导，可以得到：

由式(11)-(13)可以看出，与采用常规的直角坐标系下线性动力学模型进行仅测角相对导航不同，当在柱面动力学模型下进行ρrel和θrel、zrel演化时，基于α和β角的视线矢量与状态量之间的关系是强非线性的，所以根据α、β的变化就可以捕捉到ρrel和θrel、zrel的改变，这在一定程度上反应了相对状态的可观测性。

另外，主星位于从星低轨和高轨时，具有不同的观测效果，如图4和图5所示。

图4 低轨观测高轨示意图Fig.4 Case of low orbit observes high orbit

图5 高轨观测低轨示意图Fig.5 Case of high orbit observes low orbit

由图4和图5可以看到当主星位于低轨道时，从主星观测从星的视线矢量与从星的轨道只有一个交点。而当主星位于高轨道时，观测位于低轨道的从星，则视线会在从星轨道上有两个交点（相位角不同），且当主星位于高轨道时，从星在轨道上的位置和观测角的变化关系更为复杂。也就是说主星位于低轨道时，系统具有更好的观测性。

4 基于SCKF的长航时滤波估计方案

由式(8)和(10)表示的仅测角导航系统具有强非线性，常用的扩展卡尔曼滤波器（Extended Kalman Filter, EKF）存在线性化误差累积导致的精度不高、稳定性差等问题，难以适用于强非线性系统[15]；无迹卡尔曼滤波(Unscented Kalman Filter, UKF)能够适用于非线性导航系统[16,17]，但是只有二阶精度。因此，本文采用适用于非线性系统的、精度更高的平方根容积卡尔曼滤波算法(SCKF)。

下面先简单介绍SCKF的主要计算步骤，然后给出长航时条件下的仅测角相对导航滤波方案。

4.1 SCKF滤波算法

容积卡尔曼滤波（Cubature Kalman Filter, CKF）算法是Arasaratnam和Haykin 2009年提出的一种非线性滤波算法[18]，其核心是采用三阶球面-相径容积规则近似非线性函数传递的后验均值和协方差，从数值积分的角度来进行近似高斯积分。在采用CKF对模型进行仿真时，协方差矩阵在运算过程中可能会出现非正定从而导致算法出错，而采用CKF的平方根形式SCKF，则可以避免这种问题[18]。

在CKF的平方根形式中，误差协方差的平方根Sk-1∣k-1是可用的，SCKF的算法流程如下所示：

时间更新：

1）求解容积点(i=1,2…m)

2）求解传播容积点(i=1,2…m)

3）估计状态预测值

4）估计预测误差协方差的平方根因子

其中，SQ,k-1是Qk-1的平方根因子，关系式如下：

测量更新：

1）求解容积点(i=1,2 …m)

2）求解传播容积点(i=1,2…m)

3）计算测量预测值

4）计算新的协方差矩阵平方根

其中，SR,k是Rk的平方根因子，关系式如下：

5）计算互协方差矩阵

6）计算滤波增益

7）状态的测量更新

8）误差协方差的平方根因子更新

这里将一般的三角化算法(如QR分解)表示为S=Tria(A)，其中S是下三角矩阵。矩阵A和S的关系如下：假设R是通过对AT的QR分解得到的上三角矩阵，则有S=RT。

4.2 长航时滤波方案

在导航时长较长的情况下，误差协方差的平方根形式的矩阵S会随着时间推移逐渐趋近于0，从而使得滤波增益也逐渐趋于零，那么测量更新将失去对预测状态的校正作用。也就是说，在长航时的情况下，采用常规滤波方案相对轨道状态误差最终会因为初值误差、模型噪声的累积而发散，这与系统可观测性无关，是滤波算法使然。同时，当主从星轨道存在高度差时，随着长时间的推移，主从星间的相位角之差会减小至零，根据测量模型(10)可知，当相位角之差趋于零时，视线角也将会处于奇异状态，必将使估计误差发散。

为了实现对非合作目标的长航时稳定跟踪导航，必须解决上述两个问题。有研究表明在长时间导航中滤波器定时重新启动会提升性能[19]，因此本文设计如下两步操作方案：

（1）设置测量更新开关，当相对角度过小时（具体设置由仿真经验设置）关闭测量更新，直到超出该范围时重新打开测量更新开关；

（2）设置滤波器定时重启开关，即滤波器定时重新初始化，以开关开启上一时刻的状态值作为当前时刻的滤波初值，重新设置估计误差协方差平方根形式的矩阵S。滤波器重启周期也将通过数值仿真进行优选。

5 数值仿真与分析

5.1 参数设置

本文以GEO轨道邻域的态势感知为背景设置相对运动。假设主星运行在近圆轨道上，轨道参数如表1所示。从星在主星上方70公里高的轨道上，轨道偏心率、轨道倾角、升交点赤经和近地点幅角等参数和主星相同，仅通过改变真近点角设置两种相对运动工况。第一种工况从星初始时刻的真近点角落后主星8 °，初始时刻主从星间距离大约为5900 km，仿真周期内双星之间的距离一直增大；第二种工况从星初始时刻真近点角领先主星2 °，初始时刻主从星间距离大约为1500 km，仿真周期内星间距离先减小后增大。

表1 主星的轨道参数设置Tab.1 Chief’s orbit parameter settings

仿真中选用差、中、好三种不同精度的光学相机进行算法验证，对应的测角误差标准差分别为10-3rad、10-4rad、10-5rad。参考轨道由轨道动力学模型积分得到，积分步长为1 s，总仿真时长为5个轨道周期。滤波系统协方差平方根形式的矩阵重置周期设置为1~24小时，每一种增加1小时，重置的S矩阵为初始状态与参考状态之差的绝对值所构成的对角矩阵。设定初始距离的不确定性为5 km，测量更新关闭的相对角度θrel边界条件设置为 ± 0.01 °。

此外，由于仅测角相对导航的核心问题是距离的可观测性问题，因此后续的仿真结果分析中将以相对距离估计误差的百分比作为指标。

5.2 仿真结果与分析

第一种工况仿真的相对距离估计误差百分比如图6所示，光学相机的测角误差的标准差为10-5rad，其中1~24 h表示S矩阵重置周期。由图6可知，在重置周期为1 h的条件下，仿真结束时相对距离估计误差接近20%。而重置周期为2 h时，终端时刻估计误差下降到约3%，其他情况下的估计误差均在1%以内。这里必须说明的是，图6中误差百分比曲线之所以显示出一定的发散情况并非是系统可观测性问题，而是因为两星之间的距离一直在增加，同样的测角误差对应的相对位置误差会更大。当相对距离减少时将不会出现这种情况，工况二的仿真结果将证明这个结论。

图6 工况1的相对距离估计百分比误差Fig.6 The percent error of range estimate for Case 1

不同测角精度下的终端时刻估计误差百分比统计如表2所示（5~10 h重置周期的情况）。显然，测角精度越高时导航性能越好。由表2可知，重置周期为8 h（约1/3个主星轨道周期）时估计精度最高，10-3、10-4、10-5rad的测角精度时距离估计误差分别为0.181%、0.061%和0.005%。此外，8 周期下三种精度相机的全程估计误差曲线如图7所示，可见10-5rad测角精度时估计误差收敛非常稳定，没有出现缓慢增长趋势，收敛后距离误差始终维持在星间距离的0.01%以下。

表2 工况1距离估计百分比误差统计Tab.2 The statistic of percent error for Case 1

图7 工况1：不同测角精度的估计误差（重置周期8 h）Fig.7 Case 1：Estimate error for different camera level (8 h)

第二种工况仿真的相对距离估计误差百分比如图8所示(测角精度10-5rad)。由图可知，正如工况一中分析的一样，工况二的相对距离估计误差整体上呈现收敛趋势，但是从约2个轨道周期后开始有发散增长的趋势，这是因为约经过2个周期后主星与从星的相位角之差趋近为0，触发了长航时导航滤波方案中设定的测量更新关闭开关，此时相对轨道单纯由相对轨道动力学模型进行演化。而在此之后相位角开始逐渐增大，大于设定的阈值之后测量更新开启，增长的估计误差又被重新拉回到收敛趋势。收敛后相对距离估计误差整体上在0.4%以下，对应的距离误差约为200 m以内。

图8 工况2的相对距离估计百分比误差Fig.8 The percent error of range estimate for Case 2

此外，与工况一相关，重置周期为8 h时估计精度最高，如表3所示。对应的三种精度相机的全程估计误差曲线如图9所示，可见10-5rad测角精度时估计误差收敛非常稳定。

图9 工况2：不同测角精度的估计误差（重置周期8 h）Fig.9 Case 2：Estimate error for different camera level (8 h)

表3 工况2距离估计百分比误差统计Tab.3 The statistic of percent error for Case 2

综上，采用本文设计的相对导航方案，可以实现非合作目标无源探测的长航时跟踪导航。采用10-5rad量级的测角相机时，距离估计精度可达星间距0.01%；10-4rad量级的测角相机时，距离估计精度可保持在星间距0.1%以内。

6 结 论

本文面向空间非合作目标无源探测任务，针对笛卡尔坐标系下仅测角定轨存在相对轨道状态不可观测的问题，利用柱面坐标系的轨道曲率捕获能力，建立了柱面坐标系下的相对轨道动力学模型，并基于该模型研究了仅测角导航算法，实现了基于仅有角度测量的空间非合作目标长航时跟踪定轨。在满足可观测条件时，即使感知卫星与目标之间的距离达到数千公里，长航时相对定轨误差依然可以保持在公里级至几百米的范围之内。下一步将分别针对高轨和低轨情况引入不同的摄动项，建立更加贴近工程实际的动力学模型，并研究仅测角相对导航的性能。