一种空间完全非合作目标相对位姿和惯量估计方法
阅读说明：本技术 一种空间完全非合作目标相对位姿和惯量估计方法 (Method for estimating relative pose and inertia of space complete non-cooperative target ) 是由 冯乾 侯晓磊 杨家男 潘泉 刘勇 于 2019-10-18 设计创作，主要内容包括：本发明公开了一种空间完全非合作目标相对位姿和惯量估计方法,步骤一、由两个左右间隔设置在追踪航天器上、且参数相同的工业相机实时获取非合作目标的图像信息,计算各特征点在相机坐标系下的3D位置和速度；步骤二、根据刚体运动模型,由至少三个特征点的3D位置和速度计算非合作目标的相对角速度；估计非合作目标在任意时刻的相对姿态；步骤三、将3D位置和速度以及刚体的相对姿态和相对角速度,估计非合作目标的质心位置、质心速度及特征点的相对位置；步骤四、估计非合作目标的转动惯量参数。在无需先验已知完全非合作目标的几何形状及特征点位置的前提下,可解算出完全非合作目标的质心位置、质心速度、相对姿态、相对角速度及惯量参数。(The invention discloses a method for estimating relative pose and inertia of a space complete non-cooperative target, which comprises the steps of firstly, acquiring image information of the non-cooperative target in real time by two industrial cameras which are arranged on a tracked spacecraft at intervals and have the same parameters, and calculating the 3D position and speed of each feature point under a camera coordinate system; step two, calculating the relative angular velocity of the non-cooperative target according to the rigid motion model and the 3D positions and the velocities of at least three characteristic points; estimating the relative attitude of the non-cooperative target at any moment; estimating the centroid position, the centroid speed and the relative position of the characteristic point of the non-cooperative target by using the 3D position, the 3D speed and the relative posture and the relative angular speed of the rigid body; and step four, estimating the rotational inertia parameters of the non-cooperative target. Under the premise of not knowing the geometric shape and the position of the characteristic point of the completely non-cooperative target a priori, the centroid position, the centroid speed, the relative attitude, the relative angular speed and the inertia parameter of the completely non-cooperative target can be calculated.)
【技术领域】
本发明属于导航领域技术领域，尤其涉及一种空间完全非合作目标相对位姿和惯量估计方法。
【背景技术】
近些年来，随着人类太空活动的日益频繁，空间碎片的数量急剧上升，截止到2013年1月，美国战略司令部下属的空间监测网(SSN)编目的大空间碎片(尺寸不小于10cm)数量接近15000颗，未被编目的尺寸小于10cm空间碎片(包括尺寸小于1mm的小空间碎片以及介于大、小空间碎片之间的危险碎片)更是难以估量，这些空间碎片对在轨航天器的正常运行构成严重威胁，亟需开展空间碎片的清除研究。与传统交会对接任务中的合作目标相比，故障卫星、失效航天器及空间碎片等非合作目标具有明显的“三无”特征，即无合作测量信标、无交互通讯、无模型参数，这些特征给非合作目标的相对导航和近场操作带来了很大挑战。
关于非合作目标的相对位姿和状态估计，目前多采用以下几种方法，一是基于单目视觉的迭代算法来求解非合作目标相对位姿参数，该方法假设目标的形状及几何尺寸已知，实际上是目标航天器上若干参考点在自身本体坐标系下的坐标，通过迭代方法求解当前时刻的相对位置和姿态。另一种是假设在先验已知非合作目标上若干特征点的位置和惯性张量的情况下，提出了一种基于陀螺仪、立体视觉和加速度计量测的非合作目标相对位姿估计方法，从而在缺少非合作目标航天器动态数据的情况下，对其实现精确自主相对导航。还有一种是假设在先验已知非合作目标的CAD结构前提下，采用基于模型匹配(如ICP)方法来解算出非合作目标的相对位姿参数。还有是基于双目立体视觉，采用SLAM(Simultaneous Localization and Mapping)方法来解算空间旋转非合作目标的质心位置、线性速度、相对姿态、相对角速度以及惯量参数。
相比与传统交会对接任务中的合作目标，故障卫星、失效航天器及空间碎片等完全非合作目标缺少测量信标、交互通讯及模型参数等便于导航的合作信息，导致传统适用于合作目标的导航算法失效。现有的应用于非合作目标的导航算法，要么依赖于目标的模型，要么计算量过大无法实现在线计算。上述方法要么依赖于先验已知形状及几何尺寸，或者需要先验已知目标上特征点的位置和目标的转动惯量，或需要先验已知目标的CAD模型，这些目标严格上说都不算完全非合作目标。而采用的SLAM方法来解算空间旋转非合作目标的质心位置、线性速度、相对姿态、相对角速度以及惯量参数，其计算量较大，耗时较长，只能离线计算。
【
发明内容
】本发明的目的是提供一种空间完全非合作目标相对位姿和惯量估计方法，在无需先验已知完全非合作目标的几何形状及特征点位置的前提下，可解算出完全非合作目标的质心位置、质心速度、相对姿态、相对角速度及惯量参数，为下一阶段对非合作目标进行近场操作提供有效的导航信息。
本发明采用以下技术方案：一种空间完全非合作目标相对位姿和惯量估计方法，该方法包括如下步骤：
步骤一、由两个左右间隔设置在追踪航天器上、且参数相同的工业相机实时获取非合作目标的图像信息，经计算获得非合作目标上若干特征点在左右相机上的图像位置和图像速度，然后计算各特征点在相机坐标系下的3D位置和速度；
步骤二、根据刚体运动模型，由步骤一中的至少三个特征点的3D位置和速度估计出非合作目标的相对角速度；结合前后时刻特征点的3D位置，估计出非合作目标在任意时刻的相对姿态；
步骤三、利用步骤一中的3D位置和速度以及步骤二中的刚体的相对姿态和相对角速度，结合非合作目标的质心相对运动约束方程，估计非合作目标的质心位置、质心速度及特征点的相对位置；
步骤四、估计非合作目标的转动惯量参数。
进一步地，该方法适用的条件为：非合作目标距离追踪航天器的距离小于100米，追踪航天器运动轨迹是圆形或近圆形轨道。
进一步地，在步骤一中，由针孔模型可得特征点Pi在左右相机上的图像位置为：
其中:
ηi＝[uiR viR uiL viL]T是第i个特征点在左右相机上的图像坐标，
ρi＝[ρix ρiy ρiz]T为第i个特征点在相机坐标系下的坐标，i＝1,2,…,N；
f为相机的焦距；
b为两相机间的基线宽；
当考虑实际量测中的图像噪声，图像位置量测模型为：
其中：
εi是建模为零均值，协方差为
由公式(1)及噪声模型(A)，得到特征点在相机坐标系下的3D位置估计：
其中：
对式(1)求导得图像速度：
根据公式(4)，并考虑图像噪声，求得第i个特征点在相机坐标系下的速度估计值为：
其中：
进一步地，该步骤二的具体过程如下：
在任意时刻t，非合作目标上任一特征点Pi的坐标满足以下几何位置关系：
速度满足以下关系：
其中：
ρ0(t)为在t时刻，非合作目标质心相对于相机坐标系的位置；
为在t时刻，非合作目标相对于追踪航天器的角速度在相机坐标系下坐标；
ri为非合作目标上特征点相对质心的位置在非合作目标本体下的坐标；
取非合作目标上任一特征点作为参考点，特征点为PN，定义
δρi(t)＝ρi(t)-ρN(t)，δri＝ri-rN，结合式(6)和式(7)可得：
由式(8)和(9)消去
其中：[δρi(t)×]表示向量δρi(t)对应的叉乘矩阵；
结合式(2)和(5)，可得：
有如下公式(12)可得非合作目标的相对角速度的估计值为：
其中：
设定初始时刻t0和任意时刻tk，其中，tk＝t0+kΔt，k是正整数，Δt是两次拍摄非合作目标图像的时间间隔，根据式(8)得：
定义姿态变化量
由式(15)计算任意时刻非合作目标的相对姿态估计值
进一步地，该步骤三的具体过程如下：非合作目标的相对位置用质心相对运动约束方程来描述，质心相对运动约束方程为CW方程，则：
其中，
对式(19)进行二阶泰勒离散化并忽略高阶项和噪声项，可得：
xp(tk)＝F1xp(tk-Δt) (20)；
其中：
Δt为两次拍摄非合作目标图像的时间间隔；
xp为包含非合作目标质心位置和速度在内的向量；
F1＝I6+Δt·F+1/2Δt2·F2；
设
X1(tk)＝G·X1(tk-Δt) (21)；
其中：
根据式(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)；
其中：
根据公式(2)、(5)、(12)和(15)计算得到的估计值
其中：
进一步地，该步骤四的过程如下：非合作目标在惯性坐标系下的角动量hI为：
其中：
定义
AxI＝0 (26)；
其中：
其中，
解方程(26)等价于最小化式(27)：
|| ||2表示向量的模；
定义B＝ATA；根据式(27)，凸二次型函数f(x)取得最小的条件为：
BxI＝0 (28)；
对于齐次方程(28)，给定xI的第一个分量为1，即
其中：b11是正实数；则齐次方程(28)写为：
Brxr＝-b1 (30)；
非合作目标的惯性张量满足自身的物理约束，即：
代入则：
则式(30)是满足约束式(32)的二次型方程，通过优化凸二次型函数
本发明的有益效果是：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在左右相机上的图像位置为：
其中:
ηi＝[uiR viR uiL viL]T是第i个特征点在左右相机上的图像坐标，
ρi＝[ρix ρiy ρiz]T为第i个特征点在相机坐标系下的坐标，i＝1,2,…,N；N为特征点的个数；
f为相机的焦距；
b为两相机间的基线宽；
当考虑实际量测中的图像噪声，图像位置量测模型为：
其中：
εi是建模为零均值，协方差为的高斯白噪声，I4表示4×4的单位矩阵。
由公式(1)及噪声模型(A)，得到特征点在相机坐标系下的3D位置估计：
其中：
对式(1)求导得图像速度：
根据公式(4)，并考虑图像噪声，求得第i个特征点在相机坐标系下的速度估计值为：
其中：
表示对应量的估计值。
上述步骤二的具体过程如下：
如图1所示，在任意时刻t，非合作目标上任一特征点Pi的坐标满足以下几何位置关系：
速度满足以下关系：
其中：
ρ0(t)为在t时刻，非合作目标质心相对于相机坐标系的位置；
ri为非合作目标上特征点相对质心的位置在非合作目标本体下的坐标；
取非合作目标上任一特征点作为参考点，特征点为PN，定义
δρi(t)＝ρi(t)-ρN(t)，δri＝ri-rN，结合式(6)和式(7)可得：
由式(8)和(9)消去
其中：[δρi(t)×]表示向量δρi(t)对应的叉乘矩阵；
结合式(2)和(5)，只能得到特征点位置和速度的估计值，则由式(10)可得：
由如下公式(12)可得非合作目标的相对角速度的估计值为：
其中：
由于行列式为0，即该矩阵的秩为2，为求解非合作目标的三维的相对角速度，要求特征点的个数N最小取值为3。
设定初始时刻t0和任意时刻tk，其中，tk＝t0+kΔt，k是正整数，Δt是两次拍摄非合作目标图像的时间间隔，根据式(8)得：
定义姿态变化量
由于公式(2)只能得到特征点位置的估计值，则由式(14)得：
由式(15)计算任意时刻非合作目标的相对姿态估计值
上述式(15)转化为经典的Wahba问题，采用q-method来求解。选择权重{ai},i＝1,2...,N-1，并定义如下矩阵：
其中，
给定非合作目标的初始相对姿态
上述步骤三的具体过程如下：非合作目标的相对位置用质心相对运动约束方程来描述，质心相对运动约束方程为CW方程，CW为Clohessy-Wiltshire的指代，则：
其中：
其中，
对式(19)进行二阶泰勒离散化并忽略高阶项和噪声项，可得：
xp(tk)＝F1xp(tk-Δt) (20)；
其中：
Δt为两次拍摄非合作目标图像的时间间隔；
xp为包含非合作目标质心位置和速度在内的向量；
F1＝I6+Δt·F+1/2Δt2·F2；
设
X1(tk)＝G·X1(tk-Δt) (21)；
其中：
根据式(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)；
其中：
根据公式(2)、(5)、(12)和(15)计算得到的估计值
其中：
上述步骤四的过程如下：对于故障卫星、失效航天器及空间碎片等完全非合作目标，在外太空没有主动力矩作用，因此其角动量在惯性系在守恒，非合作目标在惯性坐标系下的角动量hI为：
其中：
为从追踪航天器相机坐标系到惯性系的姿态旋转矩阵；
(12)和(15)估计得到，
定义
AxI＝0 (26)；
其中：
|| ||2表示向量的模；
定义B＝ATA；根据式(27)，二次型函数f(x)取得最小的条件为：
BxI＝0 (28)；
对于齐次方程(28)，给定xI的第一个分量为1，即
其中：b11是正实数；则齐次方程(28)写为：
Brxr＝-b1 (30)；
根据B是正定矩阵，且
代入
则式(30)是满足约束式(32)的二次型方程，通过优化凸二次型函数
实验验证：
为了验证本发明中的算法的性能，现选取空间某尺寸为3m×3m×3m的非合作目标为实验对象。实验中各仿真参数设计如下：
特征点的个数：4；
特征点相对非合作目标质心的相对位置：在区间[-1.51.5]m取随机数；
非合作目标的初始角速度：
非合作目标质心位置初始值：ρ(t0)＝[10 25 30]Tm；
非合作目标质心速度初始值：
非合作目标初始相对姿态：qct(t0)＝[0 0 0 1]T；
非合作目标加速度噪声为
仿真时长：2500s；
两次拍摄非合作目标图像的时间间隔：Δt＝1s；
时间间隔c＝50。
在仿真实验中，假设图像的提取、匹配工作已经完成，直接可得到带有量测噪声的图像位置和速度，其中噪声建模为均值为0，标准差幅值为2×10-5rad的高斯白噪声。
为了衡量所设计方法估计性能，现定义以下相对估计误差：
非合作目标相对姿态误差定义为：
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％，上述误差均在允许的范围内。通过验证可知，采用本发明中的方法可有效估计出非合作目标的相对状态，为下一阶段的空间近场操作提供所需的导航信息。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：一种无人机相对导航信息融合方法