一种非合作航天器位姿一体化估计和惯性参数确定方法

1.本发明涉及一种非合作航天器位姿一体化估计和惯性参数确定方法，主要应用于考虑视觉导航过程中目标航天器相对角速度和相对线速度测量信息缺失下的航天器位姿估计问题，属于航天器导航领域。


背景技术：

2.临近操作要求空间航天器，谨慎地与另一航天器保持相对近距离和指定相对姿态。目标航天器完整六自由度位姿信息的精确获取，显然是临近操作成功与否的先决条件。在相对视觉导航任务背景下，由于相机观测目标特征均位于偏离质心的航天器表面，忽略位姿间耦合关系导致的估计偏差尤为重大，而且该误差不能通过传统位姿分立的运动模型表征。此外仅依靠导航相机，追踪航天器不具备直接测量与目标航天器间的相对角速度和相对线速度的能力，这将导致位姿估计的精度下降，并影响在轨任务的成功执行。因此，在位姿耦合一体的框架下，为缺失相对角速度和相对线速度测量信息的目标航天器设计六自由度位姿精确估计方法，非常具有理论和工程意义。
3.针对目前考虑位姿耦合关系的航天器位姿估计方法，专利201710178159.6利用陀螺仪和加速度计误差测量模型，结合gps测量信息，搭建基于对偶四元数的惯性/gps组合卡尔曼滤波器。但是该方法由于分别采用了陀螺仪和加速度计对目标角速度和线速度进行测量，不适用于相对角速度和相对线速度测量信息缺失下的位姿估计问题。同样地，专利201810872927.2提出一种基于深度学习的空间非合作目标姿轨一体化参数估计方法，但是该方法并没有给出当目标角速度和线速度始终不可测时的位姿估计方法。在采用导航相机观测目标航天器的实际情况中，角速度和线速度无法获取将直接导致位姿估计的精度下降。
4.针对目前的航天器位姿一体化估计问题研究成果，大部分现有估计方法都是建立在目标航天器角速度和线速度可精确获取的假设上，忽视了视觉导航背景下无法直接测量目标相对速度信息。这种情况将导致位姿估计算法精度大幅下降，进而影响临近操作任务的顺利执行。


技术实现要素：

5.本发明的技术解决问题：克服现有技术的不足，解决航天器临近操作视觉导航任务中由于缺少相对角速度和相对线速度测量信息造成的位姿估计精度下降问题，提供一种非合作航天器位姿一体化估计和惯性参数确定方法，通过建立对偶四元数框架描述下的空间刚体航天器六自由度姿态运动学和动力学模型，然后选取对偶四元数矢量部分、对偶角速度矢量部分和转动惯量比值的误差量作为状态变量，推导状态方程与观测方程，并设计一种新型乘性扩展卡尔曼滤波器，解决了相对测量信息缺失下的位姿估计精度下降问题，实现在线估计目标航天器的实时位姿和惯性参数，提高在轨任务执行的精度。
6.本发明的技术解决方案为：一种非合作航天器位姿一体化估计和惯性参数确定方
法，其包括如下步骤：
7.s1：考虑传统位姿分立模型无法准确描述相对姿态运动与相对位置运动间固有耦合关系，建立对偶四元数框架描述下的空间刚体航天器六自由度运动学和姿态动力学模型；
8.s2：基于步骤s1中建立的航天器六自由度运动学和姿态动力学模型，选取目标航天器对偶四元数矢量部分、对偶角速度矢量部分和转动惯量比值的误差量作为状态变量，推导状态方程与观测方程；
9.s3：给出误差对偶四元数的状态校正方法，并基于步骤s2中推导的状态方程和观测方程，设计一种新型乘性扩展卡尔曼滤波器，实现在线估计目标航天器的实时位姿和惯性参数。
10.具体实现步骤如下：
11.第一步建立对偶四元数框架描述下的空间刚体航天器六自由度运动学和姿态动力学模型如下：
[0012][0013][0014]
式中，为目标航天器本体系相对惯性系的对偶四元数，为其相对时间的导数；称为目标航天器本体系相对惯性系的对偶角速度，被定义在惯性系下；为目标航天器相对惯性系的线速度，定义在惯性系下；式(1)等号右侧运算为对偶四元数乘法；为目标航天器本体系相对惯性坐标系的三轴旋转角速度，被定义在惯性系下；为相对时间的导数；j为转动惯量矩阵，视觉导航背景下，其惯量积不为零：
[0015][0016]
其中，j
xx
，j
yy
，j
zz
为主轴转动惯量，j
xy
，j
xz
，j
yz
为惯量积；τ为外部干扰力矩，可建模为高斯白噪声。此外在视觉导航的任务场景中，目标航天器转动惯量的真实值无法获取，仅能估计转动惯量的相对比值。选取转动惯量矩阵中的j
xx
作为基准，则式(2)改写为如下形式：
[0017][0018]
其中转动惯量的相对比值定义为：
[0019]
[0020]
则将转动惯量矩阵的剩余元素向量j5＝[j
yy
,j
zz
,j
xy
,j
xz
,j
yz
]相对j
xx
的比值，也纳入状态向量，与目标航天器的六自由度位姿一并进行估计。
[0021]
第二步，推导状态方程与观测方程。分别以和表示和j的估计值。定义误差对偶四元数、误差对偶角速度和误差转动惯量矩阵：
[0022][0023][0024][0025]
考虑六自由度运动学，将式(1)代入式(7)，得到
[0026][0027]
其中q
r
和q
d
分别为对偶四元数的实部和对偶部，而q0和q
v
分别为四元数q的标量和矢量部分，i3×3为单位矩阵。对式(9)所得结果的第二项近似处理：
[0028][0029]
忽略二阶小量，式(9)可写为：
[0030][0031]
由于对偶四元数存在两个约束条件：和其中为q
r
的共轭四元数，为的共轭对偶四元数。故只有六个参数相互独立。为避免协方差矩阵奇异，从原状态量中删除两个标量元素。对式(11)取矢量部分：
[0032][0033]
式中，为的矢量部分；对任意叉乘矩阵定义为
[0034]
考虑刚体航天器姿态动力学，将式(2)代入式(8)，得到：
[0035][0036]
其中然后分别推导对δτ和δj5的雅克比矩阵。对的偏导数计算如下：
[0037][0038]
类似地：
[0039][0040][0041]
其中，a为任意三维向量。
[0042]
线性化系统的离散状态方程和观测方程为：
[0043][0044]
z(t
k
)＝h(t
k
)x(t
k
)+v(t
k
)
ꢀꢀ
(18)
[0045]
其中，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时刻的测量噪声。然后选取状态量得到状态方程如下：
[0046][0047]
式中，
[0048]
[0049][0050][0051][0052]
由于目标航天器的相对位姿可直接测量得到，故可得观测矩阵为：
[0053]
h(t
k
)＝[i6×
6 06×
6 06×5]
ꢀꢀ
(24)
[0054]
第三步，设计乘性扩展卡尔曼滤波器。乘性扩展卡尔曼滤波器分为状态一步预测、测量更新、滤波更新和状态校正四个部分：
[0055]
(1)状态一步预测
[0056][0057][0058][0059]
其中，为k时刻对偶四元数矢部的估计值；为k
‑
1时刻对偶四元数矢部的估计值；为k
‑
1时刻对偶四元数矢部对时间导数的估计值，由式(1)计算得到；δt为时间间隔；为k时刻对偶角速度的估计值；为k
‑
1时刻对偶角速度的估计值；为k
‑
1时刻对偶角速度对时间导数的估计值，其实数部由式(2)计算，对偶部由式(19)计算；为k时刻线速度的估计值；为k
‑
1时刻线速度的估计值；为k
‑
1时刻线速度对时间导数的估计值。
[0060]
(2)测量更新
[0061]
由对偶四元数的观测值和一步预测值计算误差对偶四元数
[0062][0063]
(3)滤波更新
[0064][0065]
p
k|k
‑1＝φp
k
‑1φ
t
+q
k
‑1ꢀꢀ
(30)
[0066]
k
k
＝p
k|k
‑1h
t
(hp
k|k
‑1h
t
+r
k
)
‑1ꢀꢀ
(31)
[0067][0068]
x
k
＝x
k
‑1+k
k
(δq
v,k
‑
hx
k
‑1)
ꢀꢀ
(33)
[0069]
其中，φ
k
为k时刻状态转移矩阵；p
k|k
‑1为k时刻预测估计误差协方差阵；p
k
‑1为k时刻估计误差协方差阵；k
k
为k时刻增益矩阵；q
k
‑1为k时刻系统噪声方差矩阵；r
k
为测量方差矩阵。
[0070]
(4)状态校正
[0071]
利用前述对偶四元数的两个约束，以作为状态量的误差对偶四元数矢量计算完整误差对偶四元数
[0072][0073]
上式适用于的情形，而当时，采用下式校正状态：
[0074][0075]
其中分别为的实部和对偶部。
[0076]
由以上步骤即可在缺少相对角速度和线速度测量信息的情况下完成对目标航天器的六自由度位姿估计和惯性参数确定。
[0077]
本发明与现有技术相比的优点在于：
[0078]
(1)与现有的位姿分立运动模型相比，本发明建立耦合的相对位姿运动模型，并以对偶四元数的形式统一描述目标角速度和线速度信息，对位姿耦合关系进行刻画，实现了相对位置和相对姿态的高精度估计。
[0079]
(2)与现有的航天器六自由度位姿估计方法相比，本方案给出了目标角速度和线速度始终不可测时的位姿估计方法，避免了需要相应敏感器的限制，对于采用导航相机执行临近操作的在轨任务来说，更具有实际意义。
[0080]
(3)与现有基于深度学习的方法相比，本发明的实时性和计算效率尤为突出，具备实时解算目标航天器位姿参数的能力，降低了对庞大硬件计算能力和数据训练量的要求，具有很强的工程实现性。
附图说明
[0081]
图1为本发明基于对偶四元数的相对导航系统原理框图；
[0082]
图2为本发明一种非合作航天器位姿一体化估计和惯性参数估计方法的流程框图。
具体实施方式
[0083]
下面结合附图及实施例对本发明进行详细说明。
[0084]
图1为本发明所述的基于对偶四元数的相对导航系统原理框图，上述系统由导航相机、位姿解算模块和导航滤波算法三部分组成。首先，目标航天器三维模型已知，由导航相机拍摄目标航天器实时在轨图片，经背景移除和去噪后；然后送入位姿解算模块，由基于模型的位姿解算方法计算粗略的相对姿态和相对位置；最后由导航滤波器递推计算精确位姿和惯性参数，并将位姿参数更新到位姿解算模块中，以实现实时跟踪。
[0085]
如图2所示，本发明涉及一种非合作航天器位姿一体化估计和惯性参数确定方法，包括如下步骤：为精确刻画相对姿态运动与相对位置运动间的固有耦合关系，建立对偶四
元数框架描述下的空间刚体航天器六自由度姿态运动学和动力学模型；然后选取目标航天器对偶四元数矢量部分、对偶角速度矢量部分和转动惯量比值的误差量作为状态变量，确定状态方程与观测方程；最后给出误差对偶四元数的状态校正方法，并设计乘性扩展卡尔曼滤波器，实现在线估计目标航天器的实时位姿和惯性参数，提高在轨任务执行的精度。
[0086]
具体操作步骤如下：
[0087]
第一步，首先建立对偶四元数框架描述下的空间刚体航天器六自由度运动学和姿态动力学模型如下：
[0088][0089][0090]
式中，为目标航天器本体系相对惯性系的对偶四元数，为其相对时间的导数；称为目标航天器本体系相对惯性系的对偶角速度，被定义在惯性系下；为目标航天器相对惯性系的线速度，定义在惯性系下；式等号右侧运算为对偶四元数乘法；为目标航天器本体系相对惯性坐标系的三轴旋转角速度，被定义在惯性系下；为相对时间的导数。目标航天器初始相对姿态选为初始相对角速度选为目标航天器质心位于追踪航天器距离矢量为处，追踪航天器初始相对线速度选为j为转动惯量矩阵，视觉导航背景下，其惯量积不为零：
[0091][0092]
其中，j
xx
，j
yy
，j
zz
为主轴转动惯量，j
xy
，j
xz
，j
yz
为惯量积；τ为外部干扰力矩，可建模为高斯白噪声。此外在视觉导航的任务场景中，目标航天器转动惯量的真实值无法获取，仅能估计转动惯量的相对比值。选取转动惯量矩阵中的j
xx
作为基准，则改写式(2)为如下形式：
[0093][0094]
其中转动惯量的相对比值定义为：
[0095][0096]
则将转动惯量矩阵的剩余元素向量j5＝[j
yy
,j
zz
,j
xy
,j
xz
,j
yz
]相对j
xx
的比值，也纳
入状态向量，与目标航天器的六自由度位姿一并进行估计。转动惯量的相对比值设为：
[0097][0098]
第二步，推导状态方程与观测方程。分别以和表示和j的估计值。定义误差对偶四元数、误差对偶角速度和误差转动惯量矩阵：
[0099][0100][0101][0102]
考虑六自由度运动学，将式(1)代入式(7)，得到：
[0103][0104]
其中q
r
和q
d
分别为对偶四元数的实部和对偶部，而q0和q
v
分别为四元数q的标量和矢量部分，i3×3为单位矩阵。对式(44)所得结果第二项近似处理：
[0105][0106]
忽略二阶小量，式(44)可写为：
[0107][0108]
由于对偶四元数存在两个约束条件：其中为q
r
的共轭四元数，为的共轭对偶四元数。故只有六个参数相互独立。为避免协方差矩阵奇异，从原状态量中删除两个标量元素。对式(46)取矢量部分：
[0109][0110]
式中，为的矢量部分；对任意叉乘矩阵定义为
[0111]
考虑刚体航天器姿态动力学，将式(2)代入式(8)，得到：
[0112][0113]
其中然后分别推导对δτ和δj5的雅克比矩阵。对的偏导数计算如下：
[0114][0115]
类似地：
[0116][0117][0118]
其中，a为任意三维向量。
[0119]
线性化系统的离散状态方程和观测方程为：
[0120][0121]
z(t
k
)＝h(t
k
)x(t
k
)+v(t
k
)
ꢀꢀ
(53)
[0122]
其中，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时刻的测量噪声。然后选取状态量得到状态方程如下：
[0123][0124]
式中，
[0125][0126][0127][0128][0129]
由于目标航天器的相对位姿可直接测量得到，故可得观测矩阵为：
[0130]
h(t
k
)＝[i6×
6 06×
6 06×5]
ꢀꢀ
(59)
[0131]
第三步，设计乘性扩展卡尔曼滤波器。乘性扩展卡尔曼滤波器分为状态一步预测、测量更新、滤波更新和状态校正四个部分：
[0132]
(1)状态一步预测
[0133][0134][0135][0136]
其中，为k时刻对偶四元数矢部的估计值；为k
‑
1时刻对偶四元数矢部的估计值；为k
‑
1时刻对偶四元数矢部对时间导数的估计值，由式(1)计算得到；；为k时刻对偶角速度的估计值；为k
‑
1时刻对偶角速度的估计值；为k
‑
1时刻对偶角速度对时间导数的估计值，其实数部由式(2)计算，对偶部由式(19)计算；为k时刻线速度的估计值；为k
‑
1时刻线速度的估计值；为k
‑
1时刻线速度对时间导数的估计值；采样频率为2hz，故时间间隔设为δt＝0.5s。
[0137]
(2)测量更新
[0138]
由对偶四元数的观测值和一步预测值计算误差对偶四元数
[0139][0140]
(3)滤波更新
[0141][0142]
p
k|k
‑1＝φp
k
‑1φ
t
+q
k
‑1ꢀꢀ
(65)
[0143]
k
k
＝p
k|k
‑1h
t
(hp
k|k
‑1h
t
+r
k
)
‑1ꢀꢀ
(66)
[0144][0145]
x
k
＝x
k
‑1+k
k
(δq
v,k
‑
hx
k
‑1)
ꢀꢀ
(68)
[0146]
其中，φ
k
为k时刻状态转移矩阵；p
k|k
‑1为k时刻预测估计误差协方差阵；p
k
‑1为k时刻估计误差协方差阵；k
k
为k时刻增益矩阵；q
k
‑1为k时刻系统噪声方差矩阵；r
k
为测量方差矩阵。滤波器的初始参数选为p
17
×
17
(0)＝diag(1
×
10
‑9i3×3,1
×
10
‑6i3×3,1
×
10
‑7i3×3,1
×
10
‑6i3×3,1
×
10
‑4i5×5)，q
11
×
11
＝1
×
10
‑6i
11
×
11
，r6×6＝diag(q
noise2
×
i3×3,p
noise2
×
i3×3)。其中，姿态测量噪声q
noise
＝π/90，相对距离测量噪声p
noise
＝0.5。
[0147]
(4)状态校正
[0148]
利用前述对偶四元数的两个约束，以作为状态量的误差对偶四元数矢量计算完整误差对偶四元数
[0149][0150]
上式适用于的情形，而当时，采用下式校正状态：
[0151][0152]
其中分别为的实部和对偶部。
[0153]
由以上步骤可以实现在缺少相对角速度和线速度测量信息的情况下完成对目标航天器的六自由度位姿估计和惯性参数确定。
[0154]
本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。