[发明专利]双体小行星探测自主协同光学导航方法

权利要求书

1.双体小行星探测自主协同光学导航方法，其特征在于：包括步骤如下，

步骤1，在双小行星系统惯性坐标系下建立探测器的动力学方程；

步骤2，根据光学导航系统建立探测器的观测方程；

步骤3，设计双体小行星导航的无迹卡尔曼滤波器；

步骤4，以轨道预报给出两颗探测器位置速度和从星位置的先验估计值作为初始状态，结合动力学方程和观测方程，通过无迹卡尔曼滤波器进行估计，实时确定两颗探测器的位置速度和从星的位置，至此，实现双体小行星探测自主协同光学导航；

其中，步骤1实现方法为，

选择双小行星系统的质心作为原点建立双小行星系统惯性坐标系，选择X轴指向春分点，Z轴为沿双小行星互相旋转的角速度方向，Y轴与X轴、Z轴垂直构成右手坐标系；选择从星质心作为原点建立从星固连坐标系，x轴沿从星最小惯量轴方向，z轴沿从星的自旋轴方向，y轴与x轴、z轴垂直构成右手坐标系；

定义r_A、r_B、r_S和r_Sc代表主星、从星、太阳和探测器分别在双星系统惯性系中的位置，定义探测器相对于主星的位置矢量r_ASc＝r_Sc-r_A，探测器相对于从星的位置矢量r_BSc＝r_Sc-r_B，探测器相对于太阳的位置矢量r_SSc＝r_Sc-r_S，则探测器在双小行星系统惯性坐标系下的动力学方程表示为

在动力学方程中，μ_A为主星的引力常数，μ_B为从星的引力常数，μ_S为太阳的引力常数，^IR_B为由从星固连坐标系到双星系统惯性坐标系的转换矩阵，有

ω_B为从星绕Z轴旋转的角速度，t表示时间；

在动力学方程中(1)，U_20,22为从星的引力场，有

δr_BSc＝[x_B y_B z_B]^T是从星固连坐标系中从星和探测器之间的相对位置矢量，球谐系数C₂₀和C₂₂是与从星半长轴a_B、b_B、c_B相关的函数，θ_B和分别是纬度和经度，

在动力学方程(1)中，a_SRP为太阳辐射压力引起的加速度，有

A和m_Sc分别是探测器的横截面积和质量，C_R是反射系数，AU是1天文单位，ρ_S是1AU的太阳辐射压力；

步骤2实现方法为，

在双体小行星探测自主协同光学导航中，探测器通过导航相机和星间测量获取观测信息；选择相机的光心作为原点建立导航相机固连坐标系，Z_C轴为相机的光轴，垂直于图像平面，X_C轴、Y_C轴分别与图像平面两边平行，构成右手坐标系；选择探测器质心作为原点建立探测器本体固连坐标系，Z_b轴沿探测器的最大惯量主轴，X_b轴、Y_b轴指向其它两个惯量主轴，构成右手坐标系；

对于导航相机观测信息，小天体形心的位置在导航相机坐标系中的坐标为(X_C,Y_C,Z_C)^T，对应的成像点像素为(u,v)^T，导航相机的焦距为f；根据小孔成像原理，两点之间的关系如下：

则相机坐标系下小行星形心的单位指向矢量公式为：

则探测器观测主星和从星的视线信息分别为

其中，V_CA为主星视线信息，V_CB为从星视线信息，主星形心像素为(u_A,v_A)^T，从星形心像素为(u_B,v_B)^T；

对于星间测量，探测器采用激光测距获取两颗探测器间的相对距离，采用视觉导航系统获取两颗探测器间的角度信息；在其中任意一颗探测器的本体系下，两颗探测器间的相对位置为r_r＝(x_r,y_r,z_r)^T，相对距离为d_r，视线方位角为俯仰角为ψ_r，则d_r、ψ_r的表达式分别为：

星间测量由探测器间的相对距离、方位角和俯仰角组成，其表达式如下：

步骤3实现方法为，

对于双体小行星探测自主协同光学导航，两颗探测器分别使用相机对主星和从星进行观测，同时通过星间测量获取探测间的相对位置d_r和角度信息ψ_r，通过滤波器估计两颗探测器的位置速度和从星的位置；系统的状态方程和观测方程表示为

y＝h(x，v)＝[V_CA1 V_CB1 V_CA2 V_CB2 Z_r]^T+v (12)

其中r_sc1和r_sc2分别为两颗探测器的位置，v_sc1和v_sc2分别为两颗探测器的速度，V_CA1和V_CA2分别为两颗探测器观测主星的视线信息，V_CB1和V_CB2分别为两颗探测器观测从星的视线信息；随机变量u、v分别代表过程噪声和观测噪声，u、v均为不相关零均值高斯白噪声序列，均服从分布u:N(0,Q)、v:N(0,R)，Q为过程噪声协方差阵，R为观测误差方差阵；

对于无迹卡尔曼滤波器，首先需要结合协方差矩阵通过对状态量的无迹变换构造一系列sigma点χ_i，i＝0,1,2,L,2l：

其中χ矩阵由2l+1个向量组成，l为状态向量的维数，控制sigma点分布的参数λ＝α²(l+κ)-l，其中κ＝3-l，1e^-4≤α＜1，下标i代表矩阵的第i列；利用状态方程对sigma点进行时间更新，计算加权均值和协方差：

其中和是计算加权平均值和协方差时所用的权值，其公式如下：

其中β用于减小高阶项误差，可根据状态x分布的先验知识确定，对于高斯分布，β＝2最优；

利用观测方程和sigma点进行测量更新，计算加权均值和协方差：

计算无迹卡尔曼滤波增益，更新状态向量和协方差矩阵，实现无迹卡尔曼滤波估计：