[发明专利]一种非合作航天器位姿一体化估计和惯性参数确定方法

权利要求书

1.一种非合作航天器位姿一体化估计和惯性参数确定方法，其特征在于，包括以下步骤：

S1：建立对偶四元数框架描述下的空间刚体航天器六自由度运动学和姿态动力学模型；

S2：基于步骤S1中建立的所述空间刚体航天器六自由度运动学和姿态动力学模型，选取目标航天器对偶四元数矢量部分、对偶角速度矢量部分和转动惯量比值的误差量作为状态变量，确定状态方程与观测方程；

S3：采用误差对偶四元数的状态校正方法，并基于步骤S2中确定的状态方程和观测方程，设计一种新的乘性扩展卡尔曼滤波器，实现在线估计目标航天器的实时位姿和惯性参数；

所述步骤S1中，建立空间刚体航天器六自由度运动学和姿态动力学模型如下：

式中，为目标航天器本体系相对惯性系的对偶四元数，为其相对时间的导数；称为目标航天器本体系相对惯性系的对偶角速度，被定义在惯性系下；为目标航天器相对惯性系的线速度，定义在惯性系下；式(1)等号右侧运算为对偶四元数乘法；为目标航天器本体系相对惯性坐标系的三轴旋转角速度，被定义在惯性系下；为相对时间的导数；τ为外部干扰力矩，建模为高斯白噪声；J′定义为转动惯量的相对比值：

在视觉导航的任务场景中，目标航天器转动惯量的真实值无法获取，仅能估计转动惯量的相对比值，选取转动惯量矩阵中的J_xx作为基准；J为转动惯量矩阵，视觉导航背景下，其惯量积不为零：

其中，J_xx，J_yy，J_zz为主轴转动惯量，J_xy，J_xz，J_yz为惯量积，将转动惯量矩阵的剩余元素向量J₅＝[J_yy,J_zz,J_xy,J_xz,J_yz]相对J_xx的比值，也纳入状态向量，与目标航天器的六自由度位姿一并估计；

所述步骤S2中，所确定的状态方程和观测方程为：

Z(t_k)＝H(t_k)x(t_k)+v(t_k) (6)

其中，t_k表示k时刻；x(t_k)为k时刻的状态变量；F(t_k)为k时刻的状态矩阵；w(t_k)为k时刻的过程噪声；G(t_k)为k时刻的过程噪声矩阵；Z(t_k)为k时刻的测量向量；H(t_k)为k时刻的测量矩阵；v(t_k)为k时刻的测量噪声；状态量为其中误差对偶四元数、误差对偶角速度和误差转动惯量矩阵如下：

其中和表示和J的估计值，为的共轭；状态矩阵为：

式中，

其中，a为任意三维向量；为任意对偶四元数，q_r和q_d分别为的实部和对偶部，表示对四元数取矢量部分，过程噪声过程噪声矩阵为：

由于目标航天器的相对位姿由相机测量并解算得到，故测量矩阵为：

H(t_k)＝[I_6×6 0_6×6 0_6×5] (16)。

2.根据权利要求1所述的方法，其特征在于：步骤S3中，所述乘性扩展卡尔曼滤波器分为状态一步预测、测量更新、滤波更新和状态校正四个部分：

(1)状态一步预测

其中，为k时刻对偶四元数矢部的估计值；为k-1时刻对偶四元数矢部的估计值；为k-1时刻对偶四元数矢部对时间导数的估计值，由式(1)计算得到；Δt为时间间隔；为k时刻对偶角速度的估计值；为k-1时刻对偶角速度的估计值；为k-1时刻对偶角速度对时间导数的估计值，其实数部由式(2)计算，对偶部由式(19)计算；为k时刻线速度的估计值；为k-1时刻线速度的估计值；为k-1时刻线速度对时间导数的估计值；

(2)测量更新

由对偶四元数的观测值和一步预测值计算误差对偶四元数

(3)滤波更新

P_k|k-1＝ΦP_k-1Φ^T+Q_k-1 (22)

K_k＝P_k|k-1H^T(HP_k|k-1H^T+R_k)^-1 (23)

x_k＝x_k-1+K_k(δq_v,k-Hx_k-1) (25)

其中，Φ_k为k时刻状态转移矩阵；P_k|k-1为k时刻预测估计误差协方差阵；P_k-1为k时刻估计误差协方差阵；K_k为k时刻增益矩阵；Q_k-1为k时刻系统噪声方差矩阵；R_k为测量方差矩阵；

(4)状态校正

利用对偶四元数的两个约束(q_r)^*q_r＝1和以作为状态量的误差对偶四元数矢量计算完整误差对偶四元数

上式适用于的情形；

当时，采用下式校正状态：

其中分别为的实部和对偶部。