CN110823214A - 一种空间完全非合作目标相对位姿和惯量估计方法 - Google Patents

一种空间完全非合作目标相对位姿和惯量估计方法 Download PDF

Info

Publication number
CN110823214A
CN110823214A CN201910995034.1A CN201910995034A CN110823214A CN 110823214 A CN110823214 A CN 110823214A CN 201910995034 A CN201910995034 A CN 201910995034A CN 110823214 A CN110823214 A CN 110823214A
Authority
CN
China
Prior art keywords
cooperative target
relative
equation
estimating
inertia
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201910995034.1A
Other languages
English (en)
Other versions
CN110823214B (zh
Inventor
冯乾
侯晓磊
杨家男
潘泉
刘勇
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN201910995034.1A priority Critical patent/CN110823214B/zh
Publication of CN110823214A publication Critical patent/CN110823214A/zh
Application granted granted Critical
Publication of CN110823214B publication Critical patent/CN110823214B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/24—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for cosmonautical navigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Multimedia (AREA)
  • Astronomy & Astrophysics (AREA)
  • Image Analysis (AREA)

Abstract

本发明公开了一种空间完全非合作目标相对位姿和惯量估计方法，步骤一、由两个左右间隔设置在追踪航天器上、且参数相同的工业相机实时获取非合作目标的图像信息，计算各特征点在相机坐标系下的3D位置和速度；步骤二、根据刚体运动模型，由至少三个特征点的3D位置和速度计算非合作目标的相对角速度；估计非合作目标在任意时刻的相对姿态；步骤三、将3D位置和速度以及刚体的相对姿态和相对角速度，估计非合作目标的质心位置、质心速度及特征点的相对位置；步骤四、估计非合作目标的转动惯量参数。在无需先验已知完全非合作目标的几何形状及特征点位置的前提下，可解算出完全非合作目标的质心位置、质心速度、相对姿态、相对角速度及惯量参数。

Description

一种空间完全非合作目标相对位姿和惯量估计方法
【技术领域】
本发明属于导航领域技术领域，尤其涉及一种空间完全非合作目标相对位姿和惯量估计方法。
【背景技术】
近些年来，随着人类太空活动的日益频繁，空间碎片的数量急剧上升，截止到2013年1月，美国战略司令部下属的空间监测网(SSN)编目的大空间碎片(尺寸不小于10cm)数量接近15000颗，未被编目的尺寸小于10cm空间碎片(包括尺寸小于1mm的小空间碎片以及介于大、小空间碎片之间的危险碎片)更是难以估量，这些空间碎片对在轨航天器的正常运行构成严重威胁，亟需开展空间碎片的清除研究。与传统交会对接任务中的合作目标相比，故障卫星、失效航天器及空间碎片等非合作目标具有明显的“三无”特征，即无合作测量信标、无交互通讯、无模型参数，这些特征给非合作目标的相对导航和近场操作带来了很大挑战。
关于非合作目标的相对位姿和状态估计，目前多采用以下几种方法，一是基于单目视觉的迭代算法来求解非合作目标相对位姿参数，该方法假设目标的形状及几何尺寸已知，实际上是目标航天器上若干参考点在自身本体坐标系下的坐标，通过迭代方法求解当前时刻的相对位置和姿态。另一种是假设在先验已知非合作目标上若干特征点的位置和惯性张量的情况下，提出了一种基于陀螺仪、立体视觉和加速度计量测的非合作目标相对位姿估计方法，从而在缺少非合作目标航天器动态数据的情况下，对其实现精确自主相对导航。还有一种是假设在先验已知非合作目标的CAD结构前提下，采用基于模型匹配(如ICP)方法来解算出非合作目标的相对位姿参数。还有是基于双目立体视觉，采用SLAM(Simultaneous Localization and Mapping)方法来解算空间旋转非合作目标的质心位置、线性速度、相对姿态、相对角速度以及惯量参数。
相比与传统交会对接任务中的合作目标，故障卫星、失效航天器及空间碎片等完全非合作目标缺少测量信标、交互通讯及模型参数等便于导航的合作信息，导致传统适用于合作目标的导航算法失效。现有的应用于非合作目标的导航算法，要么依赖于目标的模型，要么计算量过大无法实现在线计算。上述方法要么依赖于先验已知形状及几何尺寸，或者需要先验已知目标上特征点的位置和目标的转动惯量，或需要先验已知目标的CAD模型，这些目标严格上说都不算完全非合作目标。而采用的SLAM方法来解算空间旋转非合作目标的质心位置、线性速度、相对姿态、相对角速度以及惯量参数，其计算量较大，耗时较长，只能离线计算。
【发明内容】
本发明的目的是提供一种空间完全非合作目标相对位姿和惯量估计方法，在无需先验已知完全非合作目标的几何形状及特征点位置的前提下，可解算出完全非合作目标的质心位置、质心速度、相对姿态、相对角速度及惯量参数，为下一阶段对非合作目标进行近场操作提供有效的导航信息。
本发明采用以下技术方案：一种空间完全非合作目标相对位姿和惯量估计方法，该方法包括如下步骤：
步骤一、由两个左右间隔设置在追踪航天器上、且参数相同的工业相机实时获取非合作目标的图像信息，经计算获得非合作目标上若干特征点在左右相机上的图像位置和图像速度，然后计算各特征点在相机坐标系下的3D位置和速度；
步骤二、根据刚体运动模型，由步骤一中的至少三个特征点的3D位置和速度估计出非合作目标的相对角速度；结合前后时刻特征点的3D位置，估计出非合作目标在任意时刻的相对姿态；
步骤三、利用步骤一中的3D位置和速度以及步骤二中的刚体的相对姿态和相对角速度，结合非合作目标的质心相对运动约束方程，估计非合作目标的质心位置、质心速度及特征点的相对位置；
步骤四、估计非合作目标的转动惯量参数。
进一步地，该方法适用的条件为：非合作目标距离追踪航天器的距离小于100米，追踪航天器运动轨迹是圆形或近圆形轨道。
进一步地，在步骤一中，由针孔模型可得特征点Pi在左右相机上的图像位置为：
Figure BDA0002239476420000031
其中:
ηi＝[uiR viR uiL viL]T是第i个特征点在左右相机上的图像坐标，
ρi＝[ρix ρiy ρiz]T为第i个特征点在相机坐标系下的坐标，i＝1,2,…,N；
f为相机的焦距；
b为两相机间的基线宽；
当考虑实际量测中的图像噪声，图像位置量测模型为：
Figure BDA0002239476420000041
其中：
Figure BDA0002239476420000042
是包含噪声的第i个特征点在左右相机上的图像坐标量测；
εi是建模为零均值，协方差为
Figure BDA0002239476420000043
的高斯白噪声，I4表示4×4的单位矩阵。
由公式(1)及噪声模型(A)，得到特征点在相机坐标系下的3D位置估计：
其中：
Figure BDA0002239476420000045
对式(1)求导得图像速度：
Figure BDA0002239476420000046
根据公式(4)，并考虑图像噪声，求得第i个特征点在相机坐标系下的速度估计值为：
Figure BDA0002239476420000047
其中：
Figure BDA0002239476420000051
Figure BDA0002239476420000052
表示对应量的估计值。
进一步地，该步骤二的具体过程如下：
在任意时刻t，非合作目标上任一特征点Pi的坐标满足以下几何位置关系：
Figure BDA00022394764200000511
速度满足以下关系：
Figure BDA0002239476420000053
其中：
ρ0(t)为在t时刻，非合作目标质心相对于相机坐标系的位置；
Figure BDA0002239476420000054
为在t时刻，非合作目标质心相对于相机坐标系的速度；
Figure BDA0002239476420000055
为在t时刻，非合作目标本体系到相机坐标系的姿态旋转矩阵；
为在t时刻，非合作目标相对于追踪航天器的角速度在相机坐标系下坐标；
ri为非合作目标上特征点相对质心的位置在非合作目标本体下的坐标；
取非合作目标上任一特征点作为参考点，特征点为PN，定义
δρi(t)＝ρi(t)-ρN(t)，δri＝ri-rN，结合式(6)和式(7)可得：
Figure BDA0002239476420000057
Figure BDA0002239476420000058
由式(8)和(9)消去
Figure BDA0002239476420000059
可得：
Figure BDA00022394764200000510
其中：[δρi(t)×]表示向量δρi(t)对应的叉乘矩阵；
结合式(2)和(5)，可得：
Figure BDA0002239476420000061
有如下公式(12)可得非合作目标的相对角速度的估计值为：
Figure BDA0002239476420000062
其中：
Figure BDA0002239476420000063
N最小取值为3；
设定初始时刻t0和任意时刻tk，其中，tk＝t0+kΔt，k是正整数，Δt是两次拍摄非合作目标图像的时间间隔，根据式(8)得：
Figure BDA0002239476420000064
定义姿态变化量
Figure BDA0002239476420000065
并在式(13)中消去ri得：
Figure BDA0002239476420000066
Figure BDA0002239476420000067
由式(15)计算任意时刻非合作目标的相对姿态估计值
Figure BDA0002239476420000068
进一步地，该步骤三的具体过程如下：非合作目标的相对位置用质心相对运动约束方程来描述，质心相对运动约束方程为CW方程，则：
Figure BDA0002239476420000069
其中，
Figure BDA00022394764200000611
为包含空间干扰力产生的加速度噪声；n为追踪航天器的平均轨道角速度；
对式(19)进行二阶泰勒离散化并忽略高阶项和噪声项，可得：
xp(tk)＝F1xp(tk-Δt) (20)；
其中：
Δt为两次拍摄非合作目标图像的时间间隔；
xp为包含非合作目标质心位置和速度在内的向量；
F1＝I6+Δt·F+1/2Δt2·F2；
设
Figure BDA0002239476420000071
为包含特征点相对于非合作目标质心的相对位置、以及质心相对于相机坐标系的位置和速度在内的向量；由公式(20)可得：
X1(tk)＝G·X1(tk-Δt) (21)；
其中：
Figure BDA0002239476420000072
I3为3×3的单位矩阵；
根据式(21)，对于间隔j·Δt，j是正整数，在指定的时间间隔c·Δt，满足
X1(tk-j·Δt)＝G-jX1(tk),k-c≤j＜k (22)；
其中：c为小于k的正整数；
由公式(6)和(7)，可得：
C(tk)X1(tk)＝Y(tk) (23)；
其中：
Figure BDA0002239476420000073
Figure BDA0002239476420000081
根据公式(2)、(5)、(12)和(15)计算得到的估计值
Figure BDA0002239476420000082
和
Figure BDA0002239476420000083
结合式(22)和(23)可求得X1(tk)的最小二乘估计结果为：
Figure BDA0002239476420000084
其中：
Figure BDA0002239476420000085
进一步地，该步骤四的过程如下：非合作目标在惯性坐标系下的角动量hI为：
Figure BDA0002239476420000086
其中：
Figure BDA0002239476420000087
则：
Figure BDA0002239476420000089
为追踪航天器的角速度；
Figure BDA00022394764200000810
为非合作目标的角速度；
Figure BDA00022394764200000811
为从非合作目标本体系到惯性系的姿态旋转矩阵；
Figure BDA00022394764200000812
为从追踪航天器相机坐标系到惯性系的姿态旋转矩阵；
定义
Figure BDA00022394764200000813
其中I*＝[Itxx Itxy Itxz Ityy Ityz Itzz]T是非合作目标转动惯量的各分量，
Figure BDA0002239476420000091
是非合作目标角动量在惯性系下各分量；根据公式(25)可得：
AxI＝0 (26)；
其中：
其中，
Figure BDA0002239476420000093
是为非合作目标角速度估计值
Figure BDA0002239476420000094
的各分量；
解方程(26)等价于最小化式(27)：
Figure BDA0002239476420000095
|| ||2表示向量的模；
定义B＝ATA；根据式(27)，凸二次型函数f(x)取得最小的条件为：
BxI＝0 (28)；
对于齐次方程(28)，给定xI的第一个分量为1，即
Figure BDA0002239476420000096
则矩阵B的分块矩阵表示如下：
Figure BDA0002239476420000097
其中：b11是正实数；则齐次方程(28)写为：
Brxr＝-b1 (30)；
非合作目标的惯性张量满足自身的物理约束，即：
Figure BDA0002239476420000101
代入则：
Figure BDA0002239476420000103
则式(30)是满足约束式(32)的二次型方程，通过优化凸二次型函数
Figure BDA0002239476420000104
求解xr。
本发明的有益效果是：1.对于完全非合作目标，首先根据工业相机获取的目标上若干特征点的图像位置和图像速度，从而计算特征点在相机坐标系下的3D位置和速度。
2.将完全非合作目标的惯量参数求解转化带有约束二次型优化问题。
3.主要采用最小二乘、q-method和二次型优化，计算量较小，可实现在线估计。
4.估计惯量参数时，考虑了惯性张量各分量之间的约束，估计结果更可靠。
【附图说明】
图1为特征点的几何关系图；
图2为非合作目标质心位置相对误差；
图3为非合作目标质心速度相对误差；
图4为非合作目标相对姿态误差；
图5为非合作目标相对角速度相对误差；
图6为非合作目标x轴转动惯量相对误差；
图7为非合作目标y轴转动惯量相对误差；
图8为非合作目标z轴转动惯量相对误差；
图9为非合作目标上某一特征点相对于非合作目标质心位置的相对误差。
【具体实施方式】
下面结合附图和具体实施方式对本发明进行详细说明。
本发明公开了一种空间完全非合作目标相对位姿和惯量估计方法，该方法包括如下步骤：
步骤一、由两个左右间隔设置在追踪航天器上、且参数相同的工业相机实时获取非合作目标的图像信息，经计算获得非合作目标上若干特征点在左右相机上的图像位置和图像速度，然后计算各特征点在相机坐标系下的3D位置和速度；
步骤二、根据刚体运动模型，由步骤一中的至少三个特征点的3D位置和速度估计出非合作目标的相对角速度；结合前后时刻特征点的3D位置，估计出非合作目标在任意时刻的相对姿态；如图1所示。
步骤三、利用步骤一中的3D位置和速度以及步骤二中的刚体的相对姿态和相对角速度，结合非合作目标的质心相对运动约束方程，估计非合作目标的质心位置、质心速度及特征点的相对位置；
步骤四、估计非合作目标的转动惯量参数。
上述方法适用的条件为：上述非合作目标距离追踪航天器的距离小于100米，追踪航天器运动轨迹是圆形或近圆形轨道。
在上述步骤一中，由针孔模型可得特征点Pi在左右相机上的图像位置为：
Figure BDA0002239476420000121
其中:
ηi＝[uiR viR uiL viL]T是第i个特征点在左右相机上的图像坐标，
ρi＝[ρix ρiy ρiz]T为第i个特征点在相机坐标系下的坐标，i＝1,2,…,N；N为特征点的个数；
f为相机的焦距；
b为两相机间的基线宽；
当考虑实际量测中的图像噪声，图像位置量测模型为：
Figure BDA0002239476420000122
其中：
Figure BDA0002239476420000131
是包含噪声的第i个特征点在左右相机上的图像坐标量测；
εi是建模为零均值，协方差为的高斯白噪声，I4表示4×4的单位矩阵。
由公式(1)及噪声模型(A)，得到特征点在相机坐标系下的3D位置估计：
其中：
Figure BDA0002239476420000134
对式(1)求导得图像速度：
Figure BDA0002239476420000135
根据公式(4)，并考虑图像噪声，求得第i个特征点在相机坐标系下的速度估计值为：
其中：
表示对应量的估计值。
上述步骤二的具体过程如下：
如图1所示，在任意时刻t，非合作目标上任一特征点Pi的坐标满足以下几何位置关系：
Figure BDA0002239476420000141
速度满足以下关系：
Figure BDA0002239476420000142
其中：
ρ0(t)为在t时刻，非合作目标质心相对于相机坐标系的位置；
Figure BDA0002239476420000143
为在t时刻，非合作目标质心相对于相机坐标系的速度；
Figure BDA0002239476420000144
为在t时刻，非合作目标本体系到相机坐标系的姿态旋转矩阵；
Figure BDA0002239476420000145
为在t时刻，非合作目标相对于追踪航天器的角速度在相机坐标系下坐标；
ri为非合作目标上特征点相对质心的位置在非合作目标本体下的坐标；
取非合作目标上任一特征点作为参考点，特征点为PN，定义
δρi(t)＝ρi(t)-ρN(t)，δri＝ri-rN，结合式(6)和式(7)可得：
Figure BDA0002239476420000146
Figure BDA0002239476420000147
由式(8)和(9)消去
Figure BDA0002239476420000148
可得：
Figure BDA0002239476420000149
其中：[δρi(t)×]表示向量δρi(t)对应的叉乘矩阵；
结合式(2)和(5)，只能得到特征点位置和速度的估计值，则由式(10)可得：
Figure BDA00022394764200001410
由如下公式(12)可得非合作目标的相对角速度的估计值为：
Figure BDA00022394764200001411
其中：
Figure BDA0002239476420000151
由于行列式为0，即该矩阵的秩为2，为求解非合作目标的三维的相对角速度，要求特征点的个数N最小取值为3。
设定初始时刻t0和任意时刻tk，其中，tk＝t0+kΔt，k是正整数，Δt是两次拍摄非合作目标图像的时间间隔，根据式(8)得：
Figure BDA0002239476420000153
定义姿态变化量
Figure BDA0002239476420000154
并在式(13)中消去ri得：
Figure BDA0002239476420000155
由于公式(2)只能得到特征点位置的估计值，则由式(14)得：
Figure BDA0002239476420000156
由式(15)计算任意时刻非合作目标的相对姿态估计值
Figure BDA0002239476420000157
上述式(15)转化为经典的Wahba问题，采用q-method来求解。选择权重{ai},i＝1,2...,N-1，并定义如下矩阵：
Figure BDA0002239476420000158
Figure BDA0002239476420000159
其中，
Figure BDA00022394764200001510
则L(B)的最大特征根对应的单位特征向量就是姿态变化量
Figure BDA00022394764200001511
所对应的四元数
Figure BDA00022394764200001512
这里，四元数q＝[q1 q2 q3 q4]T对应的姿态矩阵为：
Figure BDA0002239476420000161
给定非合作目标的初始相对姿态
Figure BDA0002239476420000162
可以任意给定，再结合相对姿态变化量
Figure BDA0002239476420000163
由式(14)计算任意时刻非合作目标的相对姿态
Figure BDA0002239476420000164
上述步骤三的具体过程如下：非合作目标的相对位置用质心相对运动约束方程来描述，质心相对运动约束方程为CW方程，CW为Clohessy-Wiltshire的指代，则：
Figure BDA0002239476420000165
其中：
Figure BDA00022394764200001611
为包含空间干扰力产生的加速度噪声，记
Figure BDA0002239476420000167
则得到式(19)：
其中，
Figure BDA0002239476420000169
Figure BDA00022394764200001610
为包含空间干扰力产生的加速度噪声；n为追踪航天器的平均轨道角速度；
对式(19)进行二阶泰勒离散化并忽略高阶项和噪声项，可得：
xp(tk)＝F1xp(tk-Δt) (20)；
其中：
Δt为两次拍摄非合作目标图像的时间间隔；
xp为包含非合作目标质心位置和速度在内的向量；
F1＝I6+Δt·F+1/2Δt2·F2；
设
Figure BDA0002239476420000171
为包含特征点相对于非合作目标质心的相对位置、以及质心相对于相机坐标系的位置和速度在内的向量；由公式(20)可得：
X1(tk)＝G·X1(tk-Δt) (21)；
其中：
Figure BDA0002239476420000172
I3为3×3的单位矩阵；
根据式(21)，对于间隔j·Δt，j是正整数，在指定的时间间隔c·Δt，满足
X1(tk-j·Δt)＝G-jX1(tk),k-c≤j＜k (22)；
其中：c为小于k的正整数；
由公式(6)和(7)，并考虑特征点位置和速度，非合作目标的相对角速度和姿态都是估计值，可得：
C(tk)X1(tk)＝Y(tk) (23)；
其中：
Figure BDA0002239476420000173
Figure BDA0002239476420000174
根据公式(2)、(5)、(12)和(15)计算得到的估计值
Figure BDA0002239476420000175
和
Figure BDA0002239476420000176
结合式(22)和(23)可求得X1(tk)的最小二乘估计结果为：
Figure BDA0002239476420000181
其中：
Figure BDA0002239476420000182
上述步骤四的过程如下：对于故障卫星、失效航天器及空间碎片等完全非合作目标，在外太空没有主动力矩作用，因此其角动量在惯性系在守恒，非合作目标在惯性坐标系下的角动量hI为：
Figure BDA0002239476420000183
其中：
Figure BDA0002239476420000184
上述追踪航天器的角速度和姿态可通过航天器本身的量测设备获得，为已知量，即
Figure BDA0002239476420000185
和
Figure BDA0002239476420000186
已知，
Figure BDA0002239476420000187
和
Figure BDA0002239476420000188
可由公式(12)和(15)估计得到，则：
Figure BDA0002239476420000189
Figure BDA00022394764200001810
为追踪航天器的角速度；
Figure BDA00022394764200001811
为非合作目标的角速度；
Figure BDA00022394764200001812
为从非合作目标本体系到惯性系的姿态旋转矩阵；
为从追踪航天器相机坐标系到惯性系的姿态旋转矩阵；
(12)和(15)估计得到，
定义
Figure BDA00022394764200001814
其中I*＝[Itxx Itxy Itxz Ityy Ityz Itzz]T是非合作目标转动惯量的各分量，
Figure BDA00022394764200001815
是非合作目标角动量在惯性系下各分量；根据公式(25)可得：
AxI＝0 (26)；
其中：
Figure BDA0002239476420000191
Figure BDA0002239476420000192
是为非合作目标角速度估计值
Figure BDA0002239476420000193
的各分量；解方程(26)等价于最小化式(27)：
Figure BDA0002239476420000194
|| ||2表示向量的模；
定义B＝ATA；根据式(27)，二次型函数f(x)取得最小的条件为：
BxI＝0 (28)；
对于齐次方程(28)，给定xI的第一个分量为1，即
Figure BDA0002239476420000195
则矩阵B的分块矩阵表示如下：
Figure BDA0002239476420000196
其中：b11是正实数；则齐次方程(28)写为：
Brxr＝-b1 (30)；
根据B是正定矩阵，且
Figure BDA0002239476420000197
可知Br也是正定的；非合作目标的惯性张量满足自身的物理约束，即：
Figure BDA0002239476420000201
代入
Figure BDA0002239476420000202
则：
Figure BDA0002239476420000203
则式(30)是满足约束式(32)的二次型方程，通过优化凸二次型函数
Figure BDA0002239476420000204
求解xr。
实验验证：
为了验证本发明中的算法的性能，现选取空间某尺寸为3m×3m×3m的非合作目标为实验对象。实验中各仿真参数设计如下：
特征点的个数：4；
特征点相对非合作目标质心的相对位置：在区间[-1.51.5]m取随机数；
非合作目标的初始角速度：
Figure BDA0002239476420000211
非合作目标质心位置初始值：ρ(t0)＝[10 25 30]Tm；
非合作目标质心速度初始值：
非合作目标初始相对姿态：qct(t0)＝[0 0 0 1]T；
非合作目标加速度噪声为
仿真时长：2500s；
两次拍摄非合作目标图像的时间间隔：Δt＝1s；
时间间隔c＝50。
在仿真实验中，假设图像的提取、匹配工作已经完成，直接可得到带有量测噪声的图像位置和速度，其中噪声建模为均值为0，标准差幅值为2×10-5rad的高斯白噪声。
为了衡量所设计方法估计性能，现定义以下相对估计误差：
Figure BDA0002239476420000214
Figure BDA0002239476420000215
Figure BDA0002239476420000216
表示对应量估计量，|| ||2表示向量的模，| |表示绝对值，D表示非合作目标尺寸，此处取3，因为这里特征点是在区间[-1.51.5]m取随机数，只取一个特征点的误差作为代表，惯量参数误差只取主转动惯量即Ixx,Iyy和Izz相对误差作为代表。
非合作目标相对姿态误差定义为：
eθ＝2cos-1(qe4)
其中，qe4是姿态误差四元数qe的标量部分，qe由获得。
从以上仿真结果图2、3、4、5、6、7、8和9所示，在非合作目标距离追踪航天器距离小于100m范围内，完全非合作目标的质心位置估计误差小于0.1％，质心速度估计误差小于2％，非合作目标姿态估计误差小于0.035rad，即2°，相对角速度估计相对误差小于3％，非合作目标的主转动惯量相对误差小于0.15％，非合作目标特征点相对于其质心的位置估计相对误差小于1.5％，上述误差均在允许的范围内。通过验证可知，采用本发明中的方法可有效估计出非合作目标的相对状态，为下一阶段的空间近场操作提供所需的导航信息。

Claims (6)

1.一种空间完全非合作目标相对位姿和惯量估计方法，其特征在于，该方法包括如下步骤：
步骤一、由两个左右间隔设置在追踪航天器上、且参数相同的工业相机实时获取非合作目标的图像信息，经计算获得非合作目标上若干特征点在左右相机上的图像位置和图像速度，然后计算各特征点在相机坐标系下的3D位置和速度；
步骤二、根据刚体运动模型，由所述步骤一中的至少三个特征点的3D位置和速度估计出非合作目标的相对角速度；结合前后时刻特征点的3D位置，估计出非合作目标在任意时刻的相对姿态；
步骤三、利用所述步骤一中的3D位置和速度以及步骤二中的刚体的相对姿态和相对角速度，结合非合作目标质心相对运动约束方程，估计非合作目标的质心位置、质心速度及特征点的相对位置；
步骤四、估计非合作目标的转动惯量参数。
2.根据权利要求1所述的一种空间完全非合作目标相对位姿和惯量估计方法，其特征在于，所述方法适用的条件为：所述非合作目标距离追踪航天器的距离小于100米，所述追踪航天器运动轨迹是圆形或近圆形轨道。
3.根据权利要求2所述的一种空间完全非合作目标相对位姿和惯量估计方法，其特征在于，在所述步骤一中，由针孔模型可得特征点Pi在左右相机上的图像位置为：
Figure FDA0002239476410000021
其中:
ηi＝[uiR viR uiL viL]T是第i个特征点在左右相机上的图像坐标，
ρi＝[ρix ρiy ρiz]T为第i个特征点在相机坐标系下的坐标，i＝1,2,…,N；
f为相机的焦距；
b为两相机间的基线宽；
当考虑实际量测中的图像噪声，图像位置量测模型为：
Figure FDA0002239476410000022
其中：
是包含噪声的第i个特征点在左右相机上的图像坐标量测；
εi是建模为零均值，协方差为
Figure FDA0002239476410000024
的高斯白噪声，I4表示4×4的单位矩阵；
由公式(1)及噪声模型(A)，得到特征点在相机坐标系下的3D位置估计：
Figure FDA0002239476410000031
其中：
Figure FDA0002239476410000032
对式(1)求导得图像速度：
Figure FDA0002239476410000033
根据公式(4)，并考虑图像噪声，求得第i个特征点在相机坐标系下的速度估计值为：
Figure FDA0002239476410000034
其中：
Figure FDA0002239476410000035
Figure FDA0002239476410000036
表示对应量的估计值。
4.根据权利要求2或3所述的一种空间完全非合作目标相对位姿和惯量估计方法，其特征在于，所述步骤二的具体过程如下：
在任意时刻t，非合作目标上任一特征点Pi的坐标满足以下几何位置关系：
Figure FDA0002239476410000037
速度满足以下关系：
其中：
ρ0(t)为在t时刻，非合作目标质心相对于相机坐标系的位置；
Figure FDA0002239476410000041
为在t时刻，非合作目标质心相对于相机坐标系的速度；
Figure FDA0002239476410000042
为在t时刻，非合作目标本体系到相机坐标系的姿态旋转矩阵；
为在t时刻，非合作目标相对于追踪航天器的角速度在相机坐标系下坐标；
ri为非合作目标上特征点相对质心的位置在非合作目标本体下的坐标；
取非合作目标上任一特征点作为参考点，特征点为PN，定义δρi(t)＝ρi(t)-ρN(t)，δri＝ri-rN，结合式(6)和式(7)可得：
Figure FDA0002239476410000044
Figure FDA0002239476410000045
由式(8)和(9)消去
Figure FDA0002239476410000046
可得：
Figure FDA0002239476410000047
其中：[δρi(t)×]表示向量δρi(t)对应的叉乘矩阵；
结合式(2)和(5)，可得：
Figure FDA0002239476410000048
由如下公式(12)可得非合作目标的相对角速度估计值为：
Figure FDA0002239476410000049
其中：
N最小取值为3；
设定初始时刻t0和任意时刻tk，其中，tk＝t0+kΔt，k是正整数，Δt是两次拍摄非合作目标图像的时间间隔，根据式(8)得：
定义姿态变化量
Figure FDA0002239476410000052
并在式(13)中消去ri得：
Figure FDA0002239476410000053
Figure FDA0002239476410000054
由式(15)计算任意时刻非合作目标的相对姿态估计值
Figure FDA0002239476410000055
5.根据权利要求2或3所述的一种空间完全非合作目标相对位姿和惯量估计方法，其特征在于，所述步骤三的具体过程如下：非合作目标的相对位置用质心相对运动约束方程来描述，所述质心相对运动约束方程为CW方程，则：
其中，
Figure FDA0002239476410000057
为包含空间干扰力产生的加速度噪声；n为追踪航天器的平均轨道角速度；
对式(19)进行二阶泰勒离散化并忽略高阶项和噪声项，可得：
xp(tk)＝F1xp(tk-Δt) (20)；
其中：
Δt为两次拍摄非合作目标图像的时间间隔；
xp为包含非合作目标质心位置和速度在内的向量；
F1＝I6+Δt·F+1/2Δt2·F2；
设为包含特征点相对于非合作目标质心的相对位置、以及质心相对于相机坐标系的位置和速度在内的向量；由公式(20)可得：
X1(tk)＝G·X1(tk-Δt) (21)；
其中：
Figure FDA0002239476410000061
I3为3×3的单位矩阵；
根据式(21)，对于间隔j·Δt，j是正整数，在指定的时间间隔c·Δt，满足
X1(tk-j·Δt)＝G-jX1(tk),k-c≤j＜k (22)；
其中：c为小于k的正整数；
由公式(6)和(7)，可得：
C(tk)X1(tk)＝Y(tk) (23)；
其中：
Figure FDA0002239476410000063
根据公式(2)、(5)、(12)和(15)计算得到的估计值
Figure FDA0002239476410000064
和
Figure FDA0002239476410000065
结合式(22)和(23)可求得X1(tk)的最小二乘估计结果为：
Figure FDA0002239476410000066
其中：
Figure FDA0002239476410000071
6.根据权利要求2或3所述的一种空间完全非合作目标相对位姿和惯量估计方法，其特征在于，所述步骤四的过程如下：所述非合作目标在惯性坐标系下的角动量hI为：
Figure FDA0002239476410000072
其中：
Figure FDA0002239476410000073
则：
Figure FDA0002239476410000074
Figure FDA0002239476410000075
为追踪航天器的角速度；
Figure FDA0002239476410000076
为非合作目标的角速度；
Figure FDA0002239476410000077
为从非合作目标本体系到惯性系的姿态旋转矩阵；
Figure FDA0002239476410000078
为从追踪航天器相机坐标系到惯性系的姿态旋转矩阵；
定义
Figure FDA0002239476410000079
其中I*＝[Itxx Itxy Itxz Ityy Ityz Itzz]T是非合作目标转动惯量的各分量，
Figure FDA00022394764100000710
是非合作目标角动量在惯性系下各分量；根据公式(25)可得：
AxI＝0 (26)；
其中：
Figure FDA00022394764100000711
Figure FDA00022394764100000712
是非合作目标角速度估计值
Figure FDA00022394764100000713
的各分量；解方程(26)等价于最小化式(27)：
Figure FDA0002239476410000081
|| ||2表示向量的模；
定义B＝ATA；根据式(27)，凸二次型函数f(x)取得最小的条件为：
BxI＝0 (28)；
对于齐次方程(28)，给定xI的第一个分量为1，即
Figure FDA0002239476410000082
则矩阵B的分块矩阵表示如下：
Figure FDA0002239476410000083
其中：b11是正实数；则齐次方程(28)写为：
Brxr＝-b1 (30)；
非合作目标的惯性张量满足自身的物理约束，即：
Figure FDA0002239476410000084
代入
Figure FDA0002239476410000085
则：
则式(30)是满足约束式(32)的二次型方程，通过优化凸二次型函数
Figure FDA0002239476410000092
求解xr。
CN201910995034.1A 2019-10-18 2019-10-18 一种空间完全非合作目标相对位姿和惯量估计方法 Active CN110823214B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910995034.1A CN110823214B (zh) 2019-10-18 2019-10-18 一种空间完全非合作目标相对位姿和惯量估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910995034.1A CN110823214B (zh) 2019-10-18 2019-10-18 一种空间完全非合作目标相对位姿和惯量估计方法

Publications (2)

Publication Number Publication Date
CN110823214A true CN110823214A (zh) 2020-02-21
CN110823214B CN110823214B (zh) 2021-05-25

Family

ID=69549659

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910995034.1A Active CN110823214B (zh) 2019-10-18 2019-10-18 一种空间完全非合作目标相对位姿和惯量估计方法

Country Status (1)

Country Link
CN (1) CN110823214B (zh)

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111504314A (zh) * 2020-04-30 2020-08-07 深圳市瑞立视多媒体科技有限公司 Imu与刚体的位姿融合方法、装置、设备及存储介质
CN111679292A (zh) * 2020-06-24 2020-09-18 昆山同孚智能技术有限公司 一种用于agv小车激光导航的相对定位方法
CN111709990A (zh) * 2020-05-22 2020-09-25 贵州民族大学 一种相机重定位方法和系统
CN112559959A (zh) * 2020-12-07 2021-03-26 中国西安卫星测控中心 基于特征向量的天基成像非合作目标旋转状态解算方法
CN113022894A (zh) * 2021-03-08 2021-06-25 航天科工空间工程发展有限公司 一种用于微小卫星的相对姿态确定方法
CN113135302A (zh) * 2021-03-09 2021-07-20 中国人民解放军国防科技大学 一种与机动非合作目标交会对接的方法
CN113175929A (zh) * 2021-03-12 2021-07-27 南京航空航天大学 一种基于upf的空间非合作目标相对位姿估计方法
CN113390336A (zh) * 2021-05-24 2021-09-14 武汉海微科技有限公司 一种基于机器视觉的可调式屏幕贴合对位装置和标定方法
CN114537712A (zh) * 2022-01-30 2022-05-27 西北工业大学 一种利用仅测角估计非合作机动目标机动量的方法
CN116576855A (zh) * 2023-04-13 2023-08-11 北京空间飞行器总体设计部 一种空间非合作目标自主导航的观测数据自主优选方法
CN116681733A (zh) * 2023-08-03 2023-09-01 南京航空航天大学 一种空间非合作目标近距离实时位姿跟踪方法

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101726296A (zh) * 2009-12-22 2010-06-09 哈尔滨工业大学 空间机器人视觉测量、路径规划、gnc一体化仿真系统
CN102759358A (zh) * 2012-03-14 2012-10-31 南京航空航天大学 基于失效卫星表面参考点的相对位姿动力学建模方法
CN104006803A (zh) * 2014-06-20 2014-08-27 中国人民解放军国防科学技术大学 自旋稳定空间飞行器转动运动参数的摄像测量方法
CN104406598A (zh) * 2014-12-11 2015-03-11 南京航空航天大学 一种基于虚拟滑模控制的非合作航天器姿态估计方法
CN106780511A (zh) * 2016-12-01 2017-05-31 上海航天控制技术研究所 基于单目视觉的慢旋非合作目标相对测量系统和方法
CN108376411A (zh) * 2018-01-16 2018-08-07 上海交通大学 一种基于双目视觉的非合作目标相对状态解算方法
CN108897029A (zh) * 2018-03-30 2018-11-27 北京空间飞行器总体设计部 非合作目标近距离相对导航视觉测量系统指标评估方法
CN108917772A (zh) * 2018-04-04 2018-11-30 北京空间飞行器总体设计部 基于序列图像的非合作目标相对导航运动估计方法
CN110081906A (zh) * 2019-03-28 2019-08-02 西北工业大学 基于吸附过程的非合作目标惯性特征参数的两步辨识方法
CN110186465A (zh) * 2019-07-03 2019-08-30 西北工业大学 一种基于单目视觉的空间非合作目标相对状态估计方法

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101726296A (zh) * 2009-12-22 2010-06-09 哈尔滨工业大学 空间机器人视觉测量、路径规划、gnc一体化仿真系统
CN102759358A (zh) * 2012-03-14 2012-10-31 南京航空航天大学 基于失效卫星表面参考点的相对位姿动力学建模方法
CN104006803A (zh) * 2014-06-20 2014-08-27 中国人民解放军国防科学技术大学 自旋稳定空间飞行器转动运动参数的摄像测量方法
CN104406598A (zh) * 2014-12-11 2015-03-11 南京航空航天大学 一种基于虚拟滑模控制的非合作航天器姿态估计方法
CN106780511A (zh) * 2016-12-01 2017-05-31 上海航天控制技术研究所 基于单目视觉的慢旋非合作目标相对测量系统和方法
CN108376411A (zh) * 2018-01-16 2018-08-07 上海交通大学 一种基于双目视觉的非合作目标相对状态解算方法
CN108897029A (zh) * 2018-03-30 2018-11-27 北京空间飞行器总体设计部 非合作目标近距离相对导航视觉测量系统指标评估方法
CN108917772A (zh) * 2018-04-04 2018-11-30 北京空间飞行器总体设计部 基于序列图像的非合作目标相对导航运动估计方法
CN110081906A (zh) * 2019-03-28 2019-08-02 西北工业大学 基于吸附过程的非合作目标惯性特征参数的两步辨识方法
CN110186465A (zh) * 2019-07-03 2019-08-30 西北工业大学 一种基于单目视觉的空间非合作目标相对状态估计方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
于浛等: "基于自适应容积卡尔曼滤波的非合作航天器", 《航空学报》 *

Cited By (21)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2021218731A1 (zh) * 2020-04-30 2021-11-04 深圳市瑞立视多媒体科技有限公司 Imu与刚体的位姿融合方法、装置、设备及存储介质
CN111504314B (zh) * 2020-04-30 2021-11-12 深圳市瑞立视多媒体科技有限公司 Imu与刚体的位姿融合方法、装置、设备及存储介质
CN111504314A (zh) * 2020-04-30 2020-08-07 深圳市瑞立视多媒体科技有限公司 Imu与刚体的位姿融合方法、装置、设备及存储介质
CN111709990A (zh) * 2020-05-22 2020-09-25 贵州民族大学 一种相机重定位方法和系统
CN111709990B (zh) * 2020-05-22 2023-06-20 贵州民族大学 一种相机重定位方法和系统
CN111679292B (zh) * 2020-06-24 2023-04-07 昆山同日智能技术有限公司 一种用于agv小车激光导航的相对定位方法
CN111679292A (zh) * 2020-06-24 2020-09-18 昆山同孚智能技术有限公司 一种用于agv小车激光导航的相对定位方法
CN112559959A (zh) * 2020-12-07 2021-03-26 中国西安卫星测控中心 基于特征向量的天基成像非合作目标旋转状态解算方法
CN112559959B (zh) * 2020-12-07 2023-11-07 中国西安卫星测控中心 基于特征向量的天基成像非合作目标旋转状态解算方法
CN113022894A (zh) * 2021-03-08 2021-06-25 航天科工空间工程发展有限公司 一种用于微小卫星的相对姿态确定方法
CN113135302A (zh) * 2021-03-09 2021-07-20 中国人民解放军国防科技大学 一种与机动非合作目标交会对接的方法
CN113175929B (zh) * 2021-03-12 2021-12-21 南京航空航天大学 一种基于upf的空间非合作目标相对位姿估计方法
CN113175929A (zh) * 2021-03-12 2021-07-27 南京航空航天大学 一种基于upf的空间非合作目标相对位姿估计方法
CN113390336A (zh) * 2021-05-24 2021-09-14 武汉海微科技有限公司 一种基于机器视觉的可调式屏幕贴合对位装置和标定方法
CN113390336B (zh) * 2021-05-24 2024-03-12 武汉海微科技股份有限公司 一种基于机器视觉的可调式屏幕贴合对位装置和标定方法
CN114537712A (zh) * 2022-01-30 2022-05-27 西北工业大学 一种利用仅测角估计非合作机动目标机动量的方法
CN114537712B (zh) * 2022-01-30 2023-05-23 西北工业大学 一种利用仅测角估计非合作机动目标机动量的方法
CN116576855A (zh) * 2023-04-13 2023-08-11 北京空间飞行器总体设计部 一种空间非合作目标自主导航的观测数据自主优选方法
CN116576855B (zh) * 2023-04-13 2024-08-30 北京空间飞行器总体设计部 一种空间非合作目标自主导航的观测数据自主优选方法
CN116681733A (zh) * 2023-08-03 2023-09-01 南京航空航天大学 一种空间非合作目标近距离实时位姿跟踪方法
CN116681733B (zh) * 2023-08-03 2023-11-07 南京航空航天大学 一种空间非合作目标近距离实时位姿跟踪方法

Also Published As

Publication number Publication date
CN110823214B (zh) 2021-05-25

Similar Documents

Publication Publication Date Title
CN110823214B (zh) 一种空间完全非合作目标相对位姿和惯量估计方法
CN106056664B (zh) 一种基于惯性和深度视觉的实时三维场景重构系统及方法
CN104406598B (zh) 一种基于虚拟滑模控制的非合作航天器姿态估计方法
CN104180818B (zh) 一种单目视觉里程计算装置
CN101435732B (zh) 一种基于双目光流的空间目标旋转轴及质心估计方法
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN107621266B (zh) 基于特征点跟踪的空间非合作目标相对导航方法
CN106525003A (zh) 一种基于双目视觉的姿态测量方法
CN108490433A (zh) 基于序贯滤波的空时偏差联合估计与补偿方法及系统
Li et al. Rock modeling and matching for autonomous long‐range Mars rover localization
Sun et al. Adaptive sensor data fusion in motion capture
CN106504275A (zh) 一种惯性定位与点云配准耦合互补的实时三维重建方法
CN109093620A (zh) 一种双目相机辅助的空间非合作目标动力学参数辨识方法
Panahandeh et al. IMU-camera self-calibration using planar mirror reflection
Kehoe et al. State estimation using optical flow from parallax-weighted feature tracking
Peretroukhin et al. Optimizing camera perspective for stereo visual odometry
Fua et al. Markerless full body shape and motion capture from video sequences
Gardner et al. Pose and motion estimation of free-flying objects: Aerodynamics, constrained filtering, and graph-based feature tracking
Birbach Accuracy analysis of camera-inertial sensor-based ball trajectory prediction
Lavagna et al. Uncooperative objects pose, motion and inertia tensor estimation via stereovision
JP6281938B2 (ja) データ処理装置、データ処理方法、及びデータ処理プログラム
Liu et al. 6-DOF motion estimation using optical flow based on dual cameras
Bhanu et al. Synergism of binocular and motion stereo for passive ranging
Dong et al. Robust initialization and high-accuracy state estimation for filtering-based Visual-Inertial System
Jixiu et al. Relative Position and Attitude Estimation of Non-cooperative Spacecraft Based on Stereo Camera and Dynamics

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant