技术领域
[0001] 本
发明涉及一种基于导航信息评价的双体小行星探测轨迹优化方法，属于自主导航领域。
背景技术
[0002] 小行星探测是当前深空探测领域的研究热点。与单小行星相比，双小行
星系统的不规则引
力场、自旋以及双星相互作用等因素，导致双星系统动力学更加复杂、探测难度大，对
导航系统要求高。自主光学导航是实现双体小行星探测的关键技术之一，为实现双体小行星精准探测，需要研究双体小行星探测器自主精确导航技术，其中如何设计探测器的轨迹以便达到期望的导航性能要求，是双体小行星导航的一个关键技术，直接影响了探测器的自主
定位能力，决定了探测任务是否能够成功完成，因此基于导航信息评价的轨迹优化方法是当前科技人员关注的重点问题之一，
[0003] 在已发展的双体小行星导航方法中，在先技术[1](参见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周围的导航和
位置保持进行研究。由于该方法中的单颗/两颗探测器导航均是在主星和从星位置已知的情况下进行的，而实际过程中主星和从星星历并不准确，从星相对于主星的位置也因此不准确，导航
精度，需要对从星相对于主星的位置进行估计。
[0004] 在已发展的基于导航信息评价的轨迹优化方法中，在先技术[2](欧阳威,张洪波,郑伟.环火星自主导航系统设计及参数优化研究[J].深空探测学报,2017,4(1):43-50.)，两颗探测器在火星环绕段采用
粒子群优化方法基于可观测度优化两颗探测器的轨迹，采用
扩展卡尔曼滤波确定探测器自身的位置和速度。但该方法是在单颗天体的动力学环境下基于可观测度优化两颗探测器的轨迹，由于双体小行星动力学环境不同，观测模型也不同，该轨迹优化方法并不适用。
发明内容
[0005] 为解决双体小行星导航过程中，主星和从星星历不准确使从星相对于主星的位置不准确并影响导航精度的问题，本发明公开的基于导航信息评价的双体小行星探测轨迹优化方法要解决的技术问题是：以双体小行星探测导航系统的可观测度作为评价标准，结合双体小行星探测的轨迹约束，采用优化
算法进行探测器轨迹优化得到最优轨迹，沿最优轨迹飞行能使两颗探测器整体的位置、速度估计精度最优，并且从星位置的估计精度也最优。
[0006] 本发明目的是通过下述技术方案实现。
[0007] 本发明公开的基于导航信息评价的双体小行星探测轨迹优化方法，当主星的位置已知，从星的位置不确知，在双小行星系统惯性
坐标系下建立探测器的动力学方程；根据探测器所使用的导航系统建立主星和从星的视线观测模型和探测器间的测距测
角模型，并根据
状态方程和观测方程构建可观测性矩阵，计算可观测度；选取轨迹优化性能指标并确定双体小行星探测的轨迹约束，采用
优化算法进行轨迹优化，根据搜索到的两颗探测器最优的初始位置和速度可以得到两颗探测器最优的飞行轨迹，沿最优轨迹飞行能使两颗探测器整体的导航性能最优，并且准确地估计从星的位置。
[0008] 本发明公开的基于导航信息评价的双体小行星探测轨迹优化方法，包括步骤如下：
[0009] 步骤1，在双小行星系统惯性坐标系下建立探测器的动力学方程。
[0010] 选择双小行星系统的质心作为原点建立双小行星系统惯性坐标系，选择X轴指向春分点，Z轴为沿双小行星互相旋转的
角速度方向，Y轴与X轴、Z轴垂直构成右手坐标系。选择从星质心作为原点建立从星固连坐标系，x轴沿从星最小惯量轴方向，z轴沿从星的自旋轴方向，y轴与x轴、z轴垂直构成右手坐标系。
[0011] 定义rA、rB、rS和rSc代表主星、从星、太阳和探测器分别在双星系统惯性系中的位置，定义探测器相对于主星的位置矢量rASc＝rSc-rA，探测器相对于从星的位置矢量rBSc＝rSc-rB，探测器相对于太阳的位置矢量rSSc＝rSc-rS，则探测器在双小行星系统惯性坐标系下的动力学方程表示为
[0012]
[0013] 在动力学方程中，μA为主星的引力常数，μB为从星的引力常数，μS为太阳的引力常I数，RB为由从星固连坐标系到双星系统惯性坐标系的转换矩阵，有
[0014]
[0015] ωB为从星绕Z轴旋转的角速度，t表示时间。
[0016] 在动力学方程(1)中，U20,22为从星的引力场，有
[0017]
[0018] δrBSc＝[xB yB zB]T是从星固连坐标系中从星和探测器之间的相对位置矢量，球谐系数C20和C22是与从星半长轴aB、bB、cB相关的函数，θB和 分别是纬度和经度，
[0019] 在动力学方程(1)中，aSRP为
太阳辐射压力引起的
加速度，有
[0020]
[0021] A和mSc分别是探测器的横截面积和
质量，CR是反射系数，AU是1天文单位，ρS是1AU的太阳辐射压力。
[0022] 步骤2，根据光学导航系统建立探测器的观测方程。
[0023] 在双体小行星探测自主协同光学导航中，探测器通过导航相机和星间测量获取观测信息。选择相机的
光心作为原点建立导航相机固连坐标系，ZC轴为相机的光轴，垂直于图像平面，XC轴、YC轴分别与图像平面两边平行，构成右手坐标系。选择探测器质心作为原点建立探测器本体固连坐标系，Zb轴沿探测器的最大惯量
主轴，Xb轴、Yb轴指向其它两个惯量主轴，构成右手坐标系。
[0024] 对于导航相机观测信息，小天体形心的位置在导航相机坐标系中的坐标为(XC,YC,ZC)T，对应的成像点
像素为(u,v)T，导航相机的焦距为f。根据小孔成像原理，两点之间的关系如下
[0025]
[0026] 则相机坐标系下小行星形心的单位指向矢量公式为
[0027]
[0028] 则探测器观测主星和从星的视线信息分别为
[0029]
[0030] 其中，VCA为主星视线信息，VCB为从星视线信息，主星形心像素为(uA,vA)T，从星形T心像素为(uB,vB) 。
[0031] 对于星间测量，探测器采用激光测距获取两颗探测器间的相对距离，采用视觉导航系统获取两颗探测器间的角度信息。在其中任意一颗探测器的本体系下，两颗探测器间的相对位置为rr＝(xr,yr,zr)T，相对距离为dr，视线方位角为
俯仰角为ψr，则dr、 ψr的表达式分别为：
[0032]
[0033]
[0034]
[0035] 星间测量由探测器间的相对距离、方位角和俯仰角组成，其表达式如下[0036]
[0037] 步骤3，根据状态方程和观测方程构建可观测性矩阵，计算可观测度。
[0038] 对于双体小行星探测，两颗探测器的位置速度和从星的位置需要进行估计，两颗探测器分别对主星和从星进行视线观测，同时进行星间测量，则系统的状态方程和观测方程为
[0039]
[0040] 其中rsc1和rsc2分别为两颗探测器的位置，vsc1和vsc2分别为两颗探测器的速度，VCA1和VCA2分别为两颗探测器观测主星的视线信息，VCB1和VCB2分别为两颗探测器观测从星的视线信息。
[0041] 将状态方程和观测方程对状态量的转置求偏导，得到线性化的系统矩阵F和
观测矩阵H
[0042]
[0043]
[0044] 则系统的可观测性矩阵表示为
[0045] O＝[HT FTHT … (F14)T HT]T (15)
[0046] 以可观测性矩阵条件数的倒数作为系统的可观测度
[0047]
[0048] 其中cond(O)为矩阵O的条件数。
[0049] 步骤4，选取轨迹优化性能指标。
[0050] 采用积分型的性能指标体现两颗探测器导航的整体性能，其表示形式如下：
[0051]
[0052] 其中，t0为初始时刻，tf为终止时刻。
[0053] 步骤5，确定双体小行星探测的轨迹约束。
[0054] 对于双体小行星探测，两颗探测器不能与主星、从星相撞，即
[0055]
[0056] 其中aA、bA和cA为主星半长轴，aB、bB和cB为从星半长轴，rA为双小行星系统惯性系下主星位置，rB为双小行星系统惯性系下从星位置。
[0057] 探测器的两个视线矢量不能共线，即
[0058]
[0059] 其中，rASc1和rASc2分别为两颗探测器相对于主星的位置矢量，rBSc1和rBSc2分别为两颗探测器相对于从星的位置矢量。
[0060] 两颗探测器观测同一颗星的视线矢量不能共线，即
[0061]
[0062] 两颗探测器除了利用相机观测主星和从星外，还利用星间测量进行导航，则两颗探测器需要保持一定的间距，即
[0063] dr＞dmin (21)
[0064] 其中，dr为两颗探测器间的相对距离，dmin为两颗探测器间的最短距离。
[0065] 步骤6，采用优化算法进行轨迹优化，得到基于导航信息评价的双体小行星探测优化轨迹，沿最优轨迹飞行能使两颗探测器整体的位置、速度估计精度最优，并且从星位置的估计精度也最优。
[0066] 基于导航信息评价的双体小行星探测轨迹优化问题为选取两颗探测器的初始位置和速度，两颗探测器依照动力学方程以该初始状态进行运
动能使性能指标最小，同时满足约束条件，即：
[0067]
[0068]
[0069] 则双体小行星探测的轨迹优化为采用优化算法，搜索两颗探测器的初始位置和速度，使性能指标最小，同时满足约束条件，根据搜索到的两颗探测器最优的初始位置和速度可以得到两颗探测器最优的飞行轨迹，沿最优轨迹飞行能使两颗探测器整体的导航性能最优，并且准确地估计从星的位置。
[0070] 至此，实现基于导航信息评价的双体小行星探测轨迹优化。
[0071] 有益效果：
[0072] 本发明公开的基于导航信息评价的双体小行星探测轨迹优化方法，根据系统的状态方程和观测方程构建可观测性矩阵，以可观测性矩阵条件数的倒数作为系统的可观测度，以可观测度为性能指标并结合探测器飞行轨迹约束，对探测器轨迹进行优化，探测器沿最优轨迹飞行能使两颗探测器整体的位置、速度估计精度最优，并且从星位置的估计精度也最优。本发明具有实时性好、导航精度高的优点。
附图说明
[0073] 图1为基于导航信息评价的双体小行星探测轨迹优化方法的
流程图；
[0074] 图2为本发明实例中两颗探测器在不同坐标系下的轨迹，其中：图2a)为两颗探测器在双小行星系统惯性系下的轨迹，图2b)为两颗探测器在主星固连坐标系下的轨迹。
[0075] 图3为本发明实例中沿最优轨迹飞行时探测器导航的状态误差，其中图3a)为探测器1位置误差曲线，3b)探测器1速度误差曲线，3c)探测器2位置误差曲线，3d)探测器2速度误差曲线，3e)从星的位置估计误差曲线；
[0076] 图4为本发明实例中探测器沿优化轨迹和满足约束条件的随机轨迹飞行时导航误差的蒙特卡洛图。其中图4a)为沿优化轨迹和随机轨迹飞行时两颗探测器整体的位置误差，4b)为沿优化轨迹和随机轨迹飞行时两颗探测器整体的速度误差，4c)为沿优化轨迹和随机轨迹飞行时从星的位置误差。
具体实施方式
[0077] 为了更好地说明本发明的目的与优点，下面结合附图和实例对本发明做进一步说明。
[0079] 选择双小行星系统65803Didymos作为目标天体，表1列出了双小行星系统65803Didymos的各项参数。双体小行星绕双星系统中心旋转的轨道与双星系统绕太阳旋转的轨道在同一平面内。令导航相机
分辨率为500×500像素，视场角为90°，导航相机焦距为
20mm，相机测量误差为4×10-3rad，星间测距误差为0.1m，星间测角误差为1×10-3rad。探测器质量恒定为500kg，最大横截面面积为20m2，反射系数假定为1。
[0080] 表1双体小行星65803 Didymos的参数
[0081]
[0082]
[0083] 本实施例公开的基于导航信息评价的双体小行星探测轨迹优化方法，具体实现步骤如下：
[0084] 步骤1，在双小行星系统惯性坐标系下建立探测器的动力学方程。
[0085] 选择双小行星系统的质心作为原点建立双小行星系统惯性坐标系，选择X轴指向春分点，Z轴为沿双小行星互相旋转的角速度方向，Y轴与X轴、Z轴垂直构成右手坐标系。选择从星质心作为原点建立从星固连坐标系，x轴沿从星最小惯量轴方向，z轴沿从星的自旋轴方向，y轴与x轴、z轴垂直构成右手坐标系。
[0086] 定义rA、rB、rS和rSc代表主星、从星、太阳和探测器分别在双星系统惯性系中的位置，定义探测器相对于主星的位置矢量rASc＝rSc-rA，探测器相对于从星的位置矢量rBSc＝rSc-rB，探测器相对于太阳的位置矢量rSSc＝rSc-rS，则探测器在双小行星系统惯性坐标系下的动力学方程表示为
[0087]
[0088] 在动力学方程中，μA为主星的引力常数，μB为从星的引力常数，μS为太阳的引力常数，IRB为由从星固连坐标系到双星系统惯性坐标系的转换矩阵，有
[0089]
[0090] ωB为从星绕Z轴旋转的角速度，t表示时间。
[0091] 在动力学方程(1)中，U20,22为从星的引力场，有
[0092]
[0093] δrBSc＝[xB yB zB]T是从星固连坐标系中从星和探测器之间的相对位置矢量，球谐系数C20和C22是与从星半长轴aB、bB、cB相关的函数，θB和 分别是纬度和经度，
[0094] 在动力学方程(1)中，aSRP为太阳辐射压力引起的加速度，有
[0095]
[0096] A和mSc分别是探测器的横截面积和质量，CR是反射系数，AU是1天文单位，ρS是1AU的太阳辐射压力。
[0097] 步骤2，根据光学导航系统建立探测器的观测方程。
[0098] 在双体小行星探测自主协同光学导航中，探测器通过导航相机和星间测量获取观测信息。选择相机的光心作为原点建立导航相机固连坐标系，ZC轴为相机的光轴，垂直于图像平面，XC轴、YC轴分别与图像平面两边平行，构成右手坐标系。选择探测器质心作为原点建立探测器本体固连坐标系，Zb轴沿探测器的最大惯量主轴，Xb轴、Yb轴指向其它两个惯量主轴，构成右手坐标系。
[0099] 对于导航相机观测信息，小天体形心的位置在导航相机坐标系中的坐标为(XC,YC,ZC)T，对应的成像点像素为(u,v)T，导航相机的焦距为f。根据小孔成像原理，两点之间的关系如下
[0100]
[0101] 则相机坐标系下小行星形心的单位指向矢量公式为
[0102]
[0103] 则探测器观测主星和从星的视线信息分别为
[0104]
[0105] 其中，VCA为主星视线信息，VCB为从星视线信息，主星形心像素为(uA,vA)T，从星形心像素为(uB,vB)T。对于星间测量，探测器采用激光测距获取两颗探测器间的相对距离，采用视觉导航系统获取两颗探测器间的角度信息。在其中任意一颗探测器的本体系下，两颗探测器间的相对位置为rr＝(xr,yr,zr)T，相对距离为dr，视线方位角为 俯仰角为ψr，则dr、 ψr的表达式分别为
[0106]
[0107]
[0108]
[0109] 星间测量由探测器间的相对距离、方位角和俯仰角组成，其表达式如下：
[0110]
[0111] 步骤3，根据状态方程和观测方程构建可观测性矩阵，计算可观测度。
[0112] 对于双体小行星探测，两颗探测器分别对主星和从星进行视线观测，同时进行星间测量，估计两颗探测器的位置速度和从星的位置，则系统的状态方程和观测方程为[0113]
[0114] 其中rsc1和rsc2分别为两颗探测器位置，vsc1和vsc2分别为两颗探测器速度，VCA1和VCA2分别为两颗探测器观测主星的视线信息，VCB1和VCB2分别为两颗探测器观测从星的视线信息。将公式(12)对状态量的转置求偏导，得到线性化的系统矩阵F和观测矩阵H[0115]
[0116]
[0117] 则系统的可观测性矩阵可以表示为
[0118] O＝[HT FTHT … (F14)T HT]T (15)
[0119] 以可观测性矩阵条件数的倒数作为系统的可观测度
[0120]
[0121] 其中cond(O)为矩阵O的条件数。其公式可以表示为
[0122]
[0123] σmax和σmin分别代表矩阵O的最大奇异值和最小奇异值，λmax(OTO)为矩阵OTO的最大特征值，λmin(OTO)为矩阵OTO的最小特征值。矩阵的条件数越大，矩阵越接近于病态，因此可以用来衡量系统的可观测度。由公式(17)可知，条件数cond(O)≥1，则可观测度D的取值范围为0＜D≤1。若cond(O)趋近于无穷，则D趋近于0，系统不可观；若cond(O)＝1，则此时可观测性矩阵为
正交矩阵，可观测度D取最大值1，可观测度越大，导航系统的估计精度越高。
[0124] 步骤4，选取轨迹优化性能指标。
[0125] 采用积分型的性能指标体现两颗探测器导航的整体性能，其表示形式如下[0126]
[0127] 其中，t0为初始时刻，tf为终止时刻。
[0128] 步骤5，确定双体小行星探测的轨迹约束。
[0129] 对于双体小行星探测，两颗探测器不能与主星、从星相撞，即
[0130]
[0131] 其中aA、bA和cA为主星半长轴，aB、bB和cB为从星半长轴，rA为双小行星系统惯性系下主星位置，rB为双小行星系统惯性系下从星位置。
[0132] 探测器的两个视线矢量不能共线，即
[0133]
[0134] 其中，rASc1和rASc2分别为两颗探测器相对于主星的位置矢量，rBSc1和rBSc2分别为两颗探测器相对于从星的位置矢量。
[0135] 两颗探测器观测同一颗星的视线矢量不能共线，即
[0136]
[0137] 两颗探测器除了利用相机观测主星和从星外，还利用星间测量进行导航，则两颗探测器需要保持一定的间距，即
[0138] dr＞dmin (22)
[0139] 其中，dr为两颗探测器间的相对距离，dmin为两颗探测器间的最短距离。
[0140] 步骤6，采用优化算法进行轨迹优化，得到基于导航信息评价的双体小行星探测优化轨迹，沿最优轨迹飞行能使两颗探测器整体的位置、速度估计精度最优，并且从星位置的估计精度也最优。
[0141] 基于导航信息评价的双体小行星探测轨迹优化问题为选取两颗探测器的初始位置和速度，两颗探测器依照动力学方程以该初始状态进行运动能使性能指标最小，同时满足约束条件，即
[0142]
[0143]
[0144] 则双体小行星探测的轨迹优化为采用优化算法，搜索两颗探测器的初始位置和速度，使性能指标最小，同时满足约束条件，根据搜索到的两颗探测器最优的初始位置和速度可以得到两颗探测器最优的飞行轨迹，沿最优轨迹飞行能使两颗探测器整体的导航性能最优，并且准确地估计从星的位置。在本实施例中，采用的优化算法为
遗传算法。
[0145] 至此，实现基于导航信息评价的双体小行星探测轨迹优化。利用本发明方法对两颗探测器的飞行轨迹进行优化，采用遗传算法得到的两颗探测器初始状态与优化结果如表2所示。
[0146] 表2探测器的初始状态与优化结果
[0147]
[0148] 由优化结果可知，两颗探测器分别位于双小行星系统的L4点和L5点。选择主星质心作为原点建立主星固连坐标系，xa轴沿主星最小惯量轴方向，za轴沿主星的自旋轴方向，ya轴与xa轴、za轴垂直构成右手坐标系，则两颗探测器在双小行星系统惯性系下的轨迹和在主星固连坐标系下的轨迹如图2所示。
[0149] 对于两颗探测器的导航，采用无迹卡尔曼
滤波器进行滤波估计，确定两颗探测器的位置速度和从星的位置。系统的状态方程和观测方程表示为
[0150]
[0151] y＝h(x,v)＝[VCA1 VCB1 VCA2 VCB2 Zr]T+v
[0152] 其中rsc1和rsc2分别为两颗探测器的位置，vsc1和vsc2分别为两颗探测器的速度，VCA1和VCA2分别为两颗探测器观测主星的视线信息，VCB1和VCB2分别为两颗探测器观测从星的视线信息。随机变量u、v分别代表过程噪声和观测噪声，u、v均为不相关零均值高斯白噪声序列，均服从分布u～N(0,Q)、v～N(0,R)，Q为过程噪声协方差阵，R为观测误差方差阵。
[0153] 对于无迹卡尔曼滤波器，首先需要结合协方差矩阵 通过对状态量 的无迹变换构造一系列sigma点χi，i＝0,1,2,…,2l：
[0154]
[0155] 其中χ矩阵由2l+1个向量组成，l为状态向量的维数，控制sigma点分布的参数λ＝α2(l+κ)-l，其中κ＝3-l，α＝0.001，下标i代表矩阵的第i列。利用状态方程对sigma点进行时间更新，计算加权均值和协方差：
[0156]
[0157] 其中 和 是计算加权平均值和协方差时所用的权值，其公式如下：
[0158]
[0159] 其中β用于减小高阶项误差，可根据状态x分布的先验知识确定，对于高斯分布，β＝2最优。利用观测方程和sigma点进行测量更新，计算加权均值和协方差：
[0160]
[0161] 计算无迹卡尔曼滤波增益，更新状态向量和协方差矩阵，实现无迹卡尔曼滤波估计：
[0162]
[0163] 令探测器的初始状态估计误差均为探测器实际初始状态的20％，仿真时间与双小行星系统旋转周期相同，约为11.92小时。则在从星相对于主星位置未知的情况下，两颗探测器沿优化轨迹飞行的导航误差如图3所示，由图3可知在从星位置未知的情况下，对于优化后的两颗探测器导航，探测器1和探测器2的最终位置精度均能达到6m，速度精度均达到0.2m/s，在从星相对于主星位置未知的情况下能够准确地确定两颗探测器的位置速度和从星的位置。
[0164] 对基于优化轨迹飞行的探测器导航和随机轨迹飞行的探测器导航进行蒙特卡洛分析，判断优化轨迹的导航性能是否最优。其中随机轨迹飞行满足步骤5的约束，均进行1000次仿真运算。由于探测器飞行的仿真时间和步长均相同，通过导航误差绝对值积分的大小来判断导航精度。由于导航误差绝对值的积分量较大，所以实际通过导航误差绝对值在整个过程中的平均值来判断导航精度，对于两颗探测器的位置误差和速度误差，均采用两颗探测器误差绝对值在整个过程中的平均值之和度量整体的导航性能，则
[0165]
[0166] 其中，erx1(t)、ery1(t)、erz1(t)为当前时刻探测器1的三轴位置误差，evx1(t)、evy1(t)、evz1(t)为当前时刻探测器1的三轴速度误差，erx2(t)、ery2(t)、erz2(t)为当前时刻探测器2的三轴位置误差，evx2(t)、evy2(t)、evz2(t)为当前时刻探测器2的三轴速度误差，erbx(t)、erby(t)、erbz(t)为当前时刻从星三轴位置误差，则导航误差的蒙特卡洛图如图4所示，由图4可知，在从星位置未知的情况下，优化轨迹位置、速度误差和从星位置误差均比随机轨迹要好，沿本发明方法得到的优化轨迹飞行能使两颗探测器整体的导航性能达到最优。
[0167] 以上所述的具体描述，对发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何
修改、等同替换、改进等，均应包含在本发明的保护范围之内。
