技术领域
[0001] 本
发明涉及一种双体小行星探测自主协同光学导航方法，属于自主导航领域。
背景技术
[0002] 小行星探测是当前深空探测领域的研究热点。与单小行星相比，双小行
星系统的不规则引
力场、自旋以及双星相互作用等因素，导致双星系统动力学更加复杂、探测难度大，对
导航系统要求高。自主光学导航是实现双体小行星探测的关键技术之一，为实现双体小行星精准探测，需要研究双体小行星探测器自主精确导航技术，是当前科技人员关注的重点问题之一。
[0003] 在已发展的双体小行星导航方法中，在先技术[1](参见PeytavíG G,Andert T P,Probst A,et al.3D Imaging for Autonomous Navigation About Small-Bodies[C].AIAA/AAS Astrodynamics Specialist Conference.Long Beach,California,2016)，利用三维
激光雷达对双小
行星系统的主星进行扫描，同时结合
扩展卡尔曼滤波进行状态估计，由于双小行星系统引力场建模的不准确和扩展卡尔曼滤波本身的
缺陷，导航估计结果发散，无法有效地确定探测器的状态。
[0004] 在先技术[2](参见Torre F,Vasile M,Serra R,et al.Autonomous Navigation of a Formation of Spacecraft in the Proximity of a Binary Asteroid[C].International Symposium on Space Technology and Science,Ehime,Japan,2017)，利用地面站、激光雷达、相机和探测器间的通信获得观测信息，并采用无迹H∞滤波方法进行状态估计，对单颗或多颗探测器在双小行星系统拉格朗日点L4周围的导航和
位置保持进行研究。由于导航过程中使用了地面站的观测信息，导航的自主性和实时性不能双体小行星探测的需求。
[0005] 在先技术[3](参见Vasile M,Torre F,Serra R,et al.Angles-Only Navigation of a Formation in the Proximity of a Binary System[C].2018Space Flight Mechanics Meeting,Kissimmee,Florida,2018.)，仅利用相机和探测器间的通信获得观测信息，并采用无迹H∞滤波方法进行状态估计，对单颗/两颗探测器在双小行星系统拉格朗日点L4周围的导航和位置保持进行研究。由于该方法中的单颗/两颗探测器导航均是在主星和从星位置已知的情况下进行的，而实际过程中主星和从星星历并不准确，从星相对于主星的位置也因此不准确，导航
精度，需要对从星相对于主星的位置进行估计。
发明内容
[0006] 为解决双体小行星导航过程中，主星和从星星历不准确使从星相对于主星的位置不准确并影响导航精度的问题，本发明公开的双体小行星探测自主协同光学导航方法要解决的技术问题是：利用两颗探测器分别对主星和从星进行视线观测，同时结合探测器间的测距测
角，能够确定两颗探测器的位置速度和从星相对于主星的位置，并且具有导航精度高的优点。
[0007] 本发明目的是通过下述技术方案实现。
[0008] 本发明公开的双体小行星探测自主协同光学导航方法，当主星的位置已知，从星的位置不确知，在双小行星系统惯性
坐标系下建立探测器的动力学方程；根据探测器所使用的导航系统建立主星和从星的视线观测模型和探测器间的测距测角模型，并设计双体小行星导航的无迹卡尔曼
滤波器；以轨道预报给出两颗探测器位置速度和从星位置的先验估计值作为初始状态，结合动力学方程和观测方程，通过无迹卡尔曼滤波器进行估计，实时确定两颗探测器的位置速度和从星的位置。
[0009] 本发明公开的双体小行星探测自主协同光学导航方法，包括步骤如下：
[0010] 步骤1，在双小行星系统惯性坐标系下建立探测器的动力学方程。
[0011] 选择双小行星系统的质心作为原点建立双小行星系统惯性坐标系，选择X轴指向春分点，Z轴为沿双小行星互相旋转的
角速度方向，Y轴与X轴、Z轴垂直构成右手坐标系。选择从星质心作为原点建立从星固连坐标系，x轴沿从星最小惯量轴方向，z轴沿从星的自旋轴方向，y轴与x轴、z轴垂直构成右手坐标系。
[0012] 定义rA、rB、rS和rSc代表主星、从星、太阳和探测器分别在双星系统惯性系中的位置，定义探测器相对于主星的位置矢量rASc＝rSc-rA，探测器相对于从星的位置矢量rBSc＝rSc-rB，探测器相对于太阳的位置矢量rSSc＝rSc-rS，则探测器在双小行星系统惯性坐标系下的动力学方程表示为
[0013]
[0014] 在动力学方程中，μA为主星的引力常数，μB为从星的引力常数，μS为太阳的引力常数，IRB为由从星固连坐标系到双星系统惯性坐标系的转换矩阵，有
[0015]
[0016] ωB为从星绕Z轴旋转的角速度，t表示时间。
[0017] 在动力学方程中(1)，U20,22为从星的引力场，有
[0018]
[0019] δrBSc＝[xB yB zB]T是从星固连坐标系中从星和探测器之间的相对位置矢量，球谐系数C20和C22是与从星半长轴aB、bB、cB相关的函数，θB和 分别是纬度和经度，
[0020] 在动力学方程(1)中，aSRP为
太阳辐射压力引起的
加速度，有
[0021]
[0022] A和mSc分别是探测器的横截面积和
质量，CR是反射系数，AU是1天文单位，ρS是1AU的太阳辐射压力。
[0023] 步骤2，根据光学导航系统建立探测器的观测方程。
[0024] 在双体小行星探测自主协同光学导航中，探测器通过导航相机和星间测量获取观测信息。选择相机的
光心作为原点建立导航相机固连坐标系，ZC轴为相机的光轴，垂直于图像平面，XC轴、YC轴分别与图像平面两边平行，构成右手坐标系。选择探测器质心作为原点建立探测器本体固连坐标系，Zb轴沿探测器的最大惯量
主轴，Xb轴、Yb轴指向其它两个惯量主轴，构成右手坐标系。
[0025] 对于导航相机观测信息，小天体形心的位置在导航相机坐标系中的坐标为(XC,YC,ZC)T，对应的成像点
像素为(u,v)T，导航相机的焦距为f。根据小孔成像原理，两点之间的关系如下：
[0026]
[0027] 则相机坐标系下小行星形心的单位指向矢量公式为：
[0028]
[0029] 则探测器观测主星和从星的视线信息分别为
[0030]
[0031] 其中，VCA为主星视线信息，VCB为从星视线信息，主星形心像素为(uA,vA)T，从星形心像素为(uB,vB)T。
[0032] 对于星间测量，探测器采用激光测距获取两颗探测器间的相对距离，采用视觉导航系统获取两颗探测器间的角度信息。在其中任意一颗探测器的本体系下，两颗探测器间的相对位置为rr＝(xr,yr,zr)T，相对距离为dr，视线方位角为
俯仰角为ψr，则dr、 ψr的表达式分别为：
[0033]
[0034]
[0035]
[0036] 星间测量由探测器间的相对距离、方位角和俯仰角组成，其表达式如下：
[0037]
[0038] 步骤3，设计双体小行星导航的无迹卡尔曼滤波器。
[0039] 对于双体小行星探测自主协同光学导航，两颗探测器分别使用相机对主星和从星进行观测，同时通过星间测量获取探测间的相对位置dr和角度信息 ψr，通过滤波器估计两颗探测器的位置速度和从星的位置。系统的
状态方程和观测方程表示为
[0040]
[0041] y＝h(x,v)＝[VCA1 VCB1 VCA2 VCB2 Zr]T+v
[0042] 其中rsc1和rsc2分别为两颗探测器的位置，vsc1和vsc2分别为两颗探测器的速度，VCA1和VCA2分别为两颗探测器观测主星的视线信息，VCB1和VCB2分别为两颗探测器观测从星的视线信息。随机变量u、v分别代表过程噪声和观测噪声，u、v均为不相关零均值高斯白噪声序列，均服从分布u～N(0,Q)、v～N(0,R)，Q为过程噪声协方差阵，R为观测误差方差阵。
[0043] 对于无迹卡尔曼滤波器，首先需要结合协方差矩阵 通过对状态量 的无迹变换构造一系列sigma点χi，i＝0,1,2,…,2l：
[0044]
[0045] 其中χ矩阵由2l+1个向量组成，l为状态向量的维数，控制sigma点分布的参数λ＝α2(l+κ)-l，其中κ＝3-l，1e-4≤α＜1，下标i代表矩阵的第i列。利用状态方程对sigma点进行时间更新，计算加权均值和协方差：
[0046]
[0047] 其中 和 是计算加权平均值和协方差时所用的权值，其公式如下：
[0048]
[0049] 其中β用于减小高阶项误差，可根据状态x分布的先验知识确定，对于高斯分布，β＝2最优。利用观测方程和sigma点进行测量更新，计算加权均值和协方差：
[0050]
[0051] 计算无迹卡尔曼滤波增益，更新状态向量和协方差矩阵，实现无迹卡尔曼滤波估计：
[0052]
[0053] 步骤4，以轨道预报给出两颗探测器位置速度和从星位置的先验估计值作为初始状态，结合动力学方程和观测方程，通过无迹卡尔曼滤波器进行估计，实时确定两颗探测器的位置速度和从星的位置，至此，实现双体小行星探测自主协同光学导航。
[0054] 有益效果：
[0055] 1、本发明公开的双体小行星探测自主协同光学导航方法，以轨道预报给出两颗探测器位置速度和从星位置的先验估计值作为初始状态，结合每颗探测器动力学方程和两颗探测器对主星、从星的视线观测和星间测量，通过无迹卡尔曼滤波器进行估计，能够在从星相对于主星位置未知的情况下实时确定两颗探测器的位置速度和从星的位置，即实现双体小行星探测自主协同光学导航，具有实时性好、导航精度高的优点。
[0056] 2、本发明公开的双体小行星探测自主协同光学导航方法，利用无迹卡尔曼滤波器进行估计，不需要将非线性系统线性化，也不需要计算雅可比矩阵，具有实现简单、收敛性好、且滤波精度高的优点。
附图说明
[0057] 图1为双体小行星探测自主协同光学导航的示意图；
[0058] 图2为双体小行星探测自主协同光学导航的
流程图；
[0059] 图3为本发明步骤1中的双小行星系统惯性系下小行星与探测器的位置关系；
[0060] 图4为本发明步骤2中的导航相机观测模型；
[0061] 图5为本发明步骤2中的星间测量观测模型；
[0062] 图6为本发明实例中的双体小行星探测自主协同光学导航的状态误差，其中图6a)为探测器1的位置估计误差曲线，6b)为探测器1的速度估计误差曲线，6c)为探测器2的位置估计误差曲线，6d)为探测器2的速度估计误差曲线，6e)为从星的位置估计误差曲线。
具体实施方式
[0063] 为了更好地说明本发明的目的与优点，下面结合附图和实例对本发明做进一步说明。
[0065] 选择双小行星系统65803Didymos作为目标天体，表1列出了双小行星系统65803Didymos的各项参数。令双体小行星绕双小行星系统中心旋转的轨道均与双小行星系统绕太阳旋转的轨道在同一平面内，导航相机
分辨率为500×500像素，视场角为90°，导航相机焦距为20mm，相机测量误差为4×10-3rad，星间测距误差为0.1m，星间测角误差为1×
10-3rad。探测器质量恒定为500kg，最大横截面面积为20m2，反射系数假定为1。
[0066] 表1双体小行星65803Didymos的参数
[0067]
[0068] 令探测器1的初始位置为双小行星系统拉格朗日点L4，轨道法向量为h1＝[0 0 1]T，则探测器1的初始速度为轨道法向量与初始位置向量积；令探测器2在双小行星系统惯性坐标系下的初始位置为[1000 1000 100]Tm，轨道法向量为h2＝[0.9914 -0.0130 0.1305]T，则探测器2的初始速度为轨道法向量与初始位置向量积。仿真时间与双小行星系统旋转周期相同，约为11.92小时，在从星相对于主星位置未知的情况下，令初始状态估计误差均为实际初始状态的20％。
[0069] 本实施例公开的双体小行星探测自主协同光学导航方法，具体实现步骤如下：
[0070] 步骤1，在双小行星系统惯性坐标系下建立探测器的动力学方程。
[0071] 选择双小行星系统的质心作为原点建立双小行星系统惯性坐标系，选择X轴指向春分点，Z轴为沿双小行星互相旋转的角速度方向，Y轴与X轴、Z轴垂直构成右手坐标系。选择从星质心作为原点建立从星固连坐标系，x轴沿从星最小惯量轴方向，z轴沿从星的自旋轴方向，y轴与x轴、z轴垂直构成右手坐标系。
[0072] 定义rA、rB、rS和rSc代表主星、从星、太阳和探测器分别在双星系统惯性系中的位置，定义探测器相对于主星的位置矢量rASc＝rSc-rA，探测器相对于从星的位置矢量rBSc＝rSc-rB，探测器相对于太阳的位置矢量rSSc＝rSc-rS，如图3所示，则探测器在双小行星系统惯性坐标系下的动力学方程表示为
[0073]
[0074] 在动力学方程中，μA为主星的引力常数，μB为从星的引力常数，μS为太阳的引力常数，IRB为由从星固连坐标系到双星系统惯性坐标系的转换矩阵，有
[0075]
[0076] ωB为从星绕Z轴旋转的角速度，t表示时间。
[0077] 在动力学方程中(1)，U20,22为从星的引力场，有
[0078]
[0079] δrBSc＝[xB yB zB]T是从星固连坐标系中从星和探测器之间的相对位置矢量，球谐系数C20和C22是与从星半长轴aB、bB、cB相关的函数，θB和 分别是纬度和经度，
[0080] 在动力学方程(1)中，aSRP为太阳辐射压力引起的加速度，有
[0081]
[0082] A和mSc分别是探测器的横截面积和质量，CR是反射系数，AU是1天文单位，ρS是1AU的太阳辐射压力，ρS＝4.5605×10-6N/m2。
[0083] 步骤2，根据光学导航系统建立探测器的观测方程。
[0084] 在双体小行星探测自主协同光学导航中，探测器通过导航相机和星间测量获取观测信息。选择相机的光心作为原点建立导航相机固连坐标系，ZC轴为相机的光轴，垂直于图像平面，XC轴、YC轴分别与图像平面两边平行，构成右手坐标系。选择探测器质心作为原点建立探测器本体固连坐标系，Zb轴沿探测器的最大惯量主轴，Xb轴、Yb轴指向其它两个惯量主轴，构成右手坐标系。
[0085] 对于导航相机观测信息，小天体形心的位置在导航相机坐标系中的坐标为(XC,YC,ZC)T，对应的成像点像素为(u,v)T，导航相机的焦距为f。根据小孔成像原理，两点之间的关系如下：
[0086]
[0087] 则相机坐标系下小行星形心的单位指向矢量公式为：
[0088]
[0089] 则如图4所示，探测器观测主星和从星的视线信息分别为
[0090]
[0091] 其中，VCA为主星视线信息，VCB为从星视线信息，主星形心像素为(uA,vA)T，从星形心像素为(uB,vB)T。对于星间测量，探测器采用激光测距获取两颗探测器间的相对距离，采用视觉导航系统获取两颗探测器间的角度信息。如图5所示，在其中任意一颗探测器的本体系下，两颗探测器间的相对位置为rr＝(xr,yr,zr)T，相对距离为dr，视线方位角为 俯仰角为ψr，则dr、 ψr的表达式分别为：
[0092]
[0093]
[0094]
[0095] 星间测量由探测器间的相对距离、方位角和俯仰角组成，其表达式如下：
[0096]
[0097] 步骤3，设计双体小行星导航的无迹卡尔曼滤波器。
[0098] 对于双体小行星探测自主协同光学导航，两颗探测器分别使用相机对主星和从星进行观测，同时通过星间测量获取探测间的相对位置dr和角度信息 ψr，通过滤波器估计两颗探测器的位置速度和从星的位置。系统的状态方程和观测方程表示为
[0099]
[0100] y＝h(x,v)＝[VCA1 VCB1 VCA2 VCB2 Zr]T+v
[0101] 其中rsc1和rsc2分别为两颗探测器的位置，vsc1和vsc2分别为两颗探测器的速度，VCA1和VCA2分别为两颗探测器观测主星的视线信息，VCB1和VCB2分别为两颗探测器观测从星的视线信息。随机变量u、v分别代表过程噪声和观测噪声，u、v均为不相关零均值高斯白噪声序列，均服从分布u～N(0,Q)、v～N(0,R)，Q为过程噪声协方差阵，R为观测误差方差阵。
[0102] 对于无迹卡尔曼滤波器，首先需要结合协方差矩阵 通过对状态量 的无迹变换构造一系列sigma点χi，i＝0,1,2,…,2l：
[0103]
[0104] 其中χ矩阵由2l+1个向量组成，l为状态向量的维数，控制sigma点分布的参数λ＝α2(l+κ)-l，其中κ＝3-l，α＝0.001，下标i代表矩阵的第i列。利用状态方程对sigma点进行时间更新，计算加权均值和协方差：
[0105]
[0106] 其中 和 是计算加权平均值和协方差时所用的权值，其公式如下：
[0107]
[0108] 其中β用于减小高阶项误差，可根据状态x分布的先验知识确定，对于高斯分布，β＝2最优。利用观测方程和sigma点进行测量更新，计算加权均值和协方差：
[0109]
[0110] 计算无迹卡尔曼滤波增益，更新状态向量和协方差矩阵，实现无迹卡尔曼滤波估计：
[0111]
[0112] 步骤4，以轨道预报给出两颗探测器位置速度和从星位置的先验估计值作为初始状态，结合动力学方程和观测方程，通过无迹卡尔曼滤波器进行估计，实时确定两颗探测器的位置速度和从星的位置，至此，实现双体小行星探测自主协同光学导航。
[0113] 利用本发明方法进行导航滤波估计，其效果如图6所示，由图6可知，两颗探测器的位置确定精度最终均达到10m，速度确定精度均达到0.25m/s，从星的位置确定精度达到2m，在从星相对于主星位置未知的情况下，本发明方法能够准确地确定两颗探测器的位置速度和从星的位置。
[0114] 以上所述的具体描述，对发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何
修改、等同替换、改进等，均应包含在本发明的保护范围之内。
