[发明专利]一种基于单目视觉的空间非合作目标相对状态估计方法

权利要求书

1.一种基于单目视觉的空间非合作目标相对状态估计方法，其特征在于包括下述步骤：

步骤一：建立目标与服务航天器之间相对运动的运动学和动力学模型；

定义如下坐标系：

(1)地心惯性坐标系I：原点位于地球质心，z轴指向地球北极，x轴指向春分点，y轴由右手法则确定；

(2)LVLH坐标系H：坐标系原点位于服务航天器的质心，x轴由地心指向服务航天器质心，z轴垂直于轨道面且与轨道角动量方向一致，y轴由右手法则确定；

(2)服务航天器本体坐标系A：坐标原点固定在服务航天器质心上，坐标轴与服务航天器惯量主轴重合；

(3)目标本体坐标系B：坐标原点固定在目标质心上，坐标轴与目标的惯量主轴重合，规定最小惯量主轴为x轴，最大惯量主轴y轴；

(5)相机坐标系C：坐标原点固定在相机光心上，z轴沿相机光轴方向，x轴y轴平行于成像平面；

目标本体坐标系B相对于服务航天器本体坐标系A的相对姿态用四元数q表示，四元数q_v＝[q₁ q₂ q₃]^T为四元数向量部分，q₄为四元数的标量部分，与四元数q对应的旋转矩阵为：

其中[q×]为向量q＝[q₁ q₂ q₃]^T的反对称矩阵：

定义四元数b和q之间乘法运算如下：

定义服务航天器本体坐标A系和目标本体坐标系B的角速度在各自本体坐标系下的表示分别为目标相对于服务航天器的角速度ω表示在目标本体坐标系B下为：

相对姿态运动学方程为：

其中服务航天器角速度通过服务航天器的惯性设备测量得到，作为已知量用于模型方程中；

目标的姿态动力学方程为：

其中ω_t表示目标角速度，即为τ_t为噪声项，J_t为目标的惯量矩阵，且J_t＝diag(J_xx,J_yy,J_zz)，J_xx,J_yy,J_zz分别为目标主轴x轴、y轴、z轴的转动惯量；令则式转化为：

目标转动惯量比为常量，应满足：

其中，表示k₁关于时间的变化率，表示k2关于时间的变化率；

定义ρ₀为目标质心相对于服务航天器质心的位置向量，在LVLH坐标系下表示为[x y z]^T，考虑到服务航天器近距离接近目标时ρ₀很小，则相对位置动力学方程为：

其中：为服务航天器轨道真近点角速度，r_c为服务航天器轨道半径，p_c为半正焦弦，分别表示x,y,z的二阶导数，表示r_c的一阶导数，服务航天器轨道参数r_c、p_c、通过服务航天器轨道确定系统得到，将作为已知量用于模型方程中；

定义p₁,…,p_N分别为目标上N个特征点相对于目标质心的位置矢量，表示在目标本体系上，由目标的刚体假设有：

其中，表示[p₁^T,…,p_N^T]^T关于时间的变化率；

最终选取待估计状态则相对运动动力学模型写为：

其中：f(X)由式(5)、(7)、(8)、(9)和(10)分别得到；w(t)为系统噪声项；

步骤二：建立观测方程，其中加入目标特征点之间的约束关系方程；

定义目标上特征点i在相机坐标系下的位置矢量ρ_i为：

式中：表示由服务航天器本体坐标系到相机坐标系的旋转矩阵，是已知量；d为相机坐标系原点在服务航天器本体坐标系下的位置向量，是已知量；表示由LVLH坐标系到服务航天器本体坐标系的旋转矩阵，通过自身测量设备得到；与待估计状态q相关，由式(1)给出；

特征点p_i的图像投影坐标与ρ_i＝[x_i y_i z_i]^T关系为：

其中f_x,f_y,c_x,c_y为所用相机的内参参数；

此外考虑特征点i,j,k，定义相对特征ρ_ij为：

同理定义ρ_jk和ρ_ki，ρ_ij在图像中的投影坐标为y_ij，两者满足的关系为：

其中u_ij＝u_i-u_j,v_ij＝v_i-v_j，

考虑到矢量和ρ_ij+ρ_jk+ρ_ki＝0，结合式，特征点i,j,k之间应满足约束：

记为M(i,j,k)＝y_i,j,k；

对于目标上N个特征点进行编号1,2,…,N，以三点为一组构成约束方程加入观测方程；假设s₁,…,s_m为一种选点组合方式，覆盖所有待估计特征点，选点组合方式分别为s₁＝1,2,3，s₂＝2,3,4，…，s_N-1＝N-1,N,1；

最终观测方程为：

观测方程中：z为所有观测量，v为观测噪声，并且所有ρ_i、ρ_ij的相关分量都应用式和式进行替换，即可得到状态到观测的映射关系；

步骤三：设计滤波器，实现目标相对状态的估计；

式描述的是连续系统状态方程，先转换为离散模型，之后再进行估计，离散模型为：

式中：X(k)为k时刻状态，Δt为滤波周期，

对式和式分别表示系统状态方程和观测方程，采用迭代扩展卡尔曼滤波进行状态估计；具体估计的步骤如下：

步骤3.1：初始化：设置初始的滤波估计值和误差方差矩阵P(0)；

步骤3.2：状态预测值为：

预测误差方差阵为：

式中：P(k+1/k)表示预测误差方差矩阵，P(k/k)表示k时刻的滤波误差方差矩阵；

其中：Q_k为系统噪声方差；

步骤3.3：状态更新：

步骤3.3.1：将预测值作为迭代初值：

P(k+1/k+1)₀＝P(k+1/k) (22)

步骤3.3.2：迭代过程中，第i次迭代为：

其中R_k+1为测量噪声方差，K(k+1)_i表示第i次迭代时的增益；

其中，(r(k)_i表示第i次迭代时的观测残差，)是将第i-1的迭代结果代入观测方程(17)得到；

其中z(k+1)为k+1时刻所有观测量；

P(k+1/k+1)_i＝[I-K(k+1)_iH(k+1)_i-1]P(k+1/k+1)_i-1 (26)

步骤3.3.3：当迭代次数达到最大迭代次数或者状态迭代误差小于设定的阈值时，迭代终止，状态迭代误差定义如下：

其中||·||₂表示向量二范数；假设第n次迭代时迭代终止，将此时的迭代结果作为最终估计结果输出：

P(k+1/k+1)＝P(k+1/k+1)_n (29)

其中，即为k+1时刻的最终状态估计结果。