[发明专利]一种基于UPF的空间非合作目标相对位姿估计方法

权利要求书

1.一种基于UPF的空间非合作目标相对位姿估计方法，其特征在于，包括以下步骤：

步骤一、读取单目相机实时采集的图像，从开始帧中筛选连续两帧图像作为初始两帧图像，检测初始两帧图像上的SURF特征点，并进行SURF特征点匹配，删除离群点，得到特征匹配点对，并依据对极几何关系计算本质矩阵，分解本质矩阵后得到初始两帧图像间的相对位姿；

根据初始两帧图像间的相对位姿，利用三角测量法获取第j个SURF特征点在相机坐标系C下的初始位置其中下标j表示SURF特征点的索引，M为SURF特征点个数；

任取一SURF特征点作为目标本体坐标系的原点，建立目标本体坐标系B，目标本体坐标系固连于目标上，目标本体坐标系的指向和相机坐标系平行，记目标本体坐标系的原点在相机坐标系中的坐标为获得相机和目标间的相对姿态初始值α₀和相对位置初始值ρ₀以及第j个SURF特征点在目标本体坐标系下的三维坐标初始值

步骤二、以k表示图像采样时刻索引，跟踪匹配第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标z_j,k＝[x_j,k y_j,k]^T，其中，x_j,k是第k帧图像采样时刻目标上的第j个SURF特征点在图像物理坐标系下的水平坐标，y_j,k是第k帧图像采样时刻目标上的第j个SURF特征点在图像物理坐标系下的垂直坐标，上标T为转置；

步骤三、以目标和相机间的相对位姿向量作为状态变量，构建目标状态转移方程，以z_j,k作为实际测量值，构建测量方程，利用贝叶斯滤波预测目标和相机间的相对姿态；利用测量反演法预测目标和相机间的相对位置，根据预测得到的相对姿态和相对位置，优化目标状态转移方程；

根据测量方程和优化后的状态转移方程，将步骤一所得到的α₀，ρ₀和作为初始值，采用无迹粒子滤波UPF方法估计相机和目标间的相对位姿；

步骤三中，构建目标状态转移方程和测量方程的具体过程如下：

定义目标的状态向量s＝[α ω ρ v]^T，其中α＝[φ θ ψ]^T表示目标本体坐标系相对于相机坐标系的旋转欧拉角，ω＝[ω_x ω_y ω_z]^T表示目标在相机坐标系下的旋转角速度矢量，ρ＝[ρ_x ρ_y ρ_z]^T表示目标在相机坐标系下的位置，即目标本体坐标系原点在相机坐标系下的三维坐标，v＝[v_x v_y v_z]^T表示目标在相机坐标系下的平移线速度；其中φ，θ，ψ分别表示α绕相机坐标系的X轴、Y轴、Z轴旋转的旋转欧拉角，ω_x，ω_y，ω_z为ω的X、Y、Z三轴角速度分量，ρ_x，ρ_y，ρ_z分别为目标本体坐标系原点在相机坐标系下的X、Y、Z坐标，v_x，v_y，v_z分别表示v的X、Y、Z三轴平移线速度分量；

以下标k，k-1分别表示第k帧图像采样时刻和第k-1帧图像采样时刻，Δt表示采样时刻间隔，则根据刚体的动力学、运动学模型，目标的状态转移方程表示为：

其中，α_k，α_k-1分别表示第k帧图像采样时刻和第k-1帧图像采样时刻目标本体坐标系相对于相机坐标系的旋转欧拉角，ω_k，ω_k-1分别表示第k帧图像采样时刻和第k-1帧图像采样时刻目标在相机坐标系下的旋转角速度矢量，ρ_k、ρ_k-1分别表示第k帧图像采样时刻和第k-1帧图像采样时刻目标在相机坐标系下的位置，v_k、v_k-1分别表示第k帧图像采样时刻和第k-1帧图像采样时刻目标在相机坐标系下的平移线速度，M(α_k-1)为矩阵，该矩阵用于描述第k-1帧图像采样时刻下、目标在相机坐标系下的旋转角速度矢量ω到欧拉角速度的转换关系，其中上标·表示对时间的导数；假定目标以X-Y-Z的欧拉旋转顺序分别旋转φ，θ，ψ，已知目标在旋转前的目标本体坐标系下的旋转角速度为分别为ω^B在旋转前的目标本体坐标系下的X、Y、Z三轴角速度分量，其中R^B/C为相机坐标系到目标本体坐标系的变换矩阵，则存在

则有

其中，M(α)为矩阵，该矩阵用于描述第k帧图像采样时刻下、目标在相机坐标系下的旋转角速度矢量ω到欧拉角速度的转换关系；公式(1)给出的速度分量状态转移方程中的J为惯量矩阵，m为目标质量，r表示目标参考点到质心的位置偏移量，T_total表示作用于目标的外部总力矩，F表示作用于目标质心的总外力；在此T_total、F、r均不知，由T_total、F、r所引起的角速度和线速度变化通过引入过程噪声q_k～N(0,Q_k)来考虑，Q_k为过程噪声q_k的协方差矩阵，则(1)式改写为

s_k＝f(s_k-1)+q_k

其中，s_k，s_k-1分别表示第k帧图像采样时刻和第k-1帧图像采样时刻目标的状态向量，q_k表示第k帧图像采样时刻的过程噪声，角速度过程噪声q_ω,k、线速度过程噪声q_v,k均为零均值的高斯噪声，即q_ω,k～N(0,Q_ω,k)，q_v,k～N(0,Q_v,k)，Q_ω,k和Q_v,k分别为角速度过程噪声q_ω,k和线速度过程噪声q_v,k的协方差矩阵，f(·)为状态转移函数；目标为刚体，则目标上的SURF特征点在目标本体坐标系下的坐标不随时间变化，即分别表示第k帧图像采样时刻和第k-1帧图像采样时刻目标上的第j个SURF特征点在目标本体坐标系的三维坐标；

假设为第k帧图像采样时刻目标上的第j个SURF特征点在相机坐标系下的坐标，分别为在相机坐标系下的X、Y、Z坐标；ρ_k为第k帧图像采样时刻目标在相机坐标系下的位置，表示第k帧图像采样时刻目标本体坐标系到相机坐标系的转换矩阵；则和存在以下转换关系：

定义第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标预测值为分别为的X轴、Y轴坐标，相机模型为针孔模型，假设焦距为f_c，和存在透视投影关系，以函数g_j(·)表示如下，

定义，表示目标上的第j个SURF特征点在目标本体坐标系下的三维坐标，X^B表示目标上所有M个SURF特征点在目标本体坐标系下的三维坐标构成的矩阵，z_k＝[z_1,k z_2,k…z_M,k]^T，z_j,k为第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标的实际测量值，z_k表示第k帧图像采样时刻目标上所有M个SURF特征点图像物理坐标实际测量值所构成的矩阵；测量噪声n_k～N(0,N_k)，N_k为测量噪声n_k的协方差矩阵，函数g(·)为测量函数、表示所有M个SURF特征点透视投影函数g_j(·)的组合，结合式(4)和式(5)构建测量方程：

在步骤三中，利用测量反演法预测目标和相机间的相对位置，根据预测得到的相对姿态和相对位置，优化目标状态转移方程的具体流程如下：

首先根据式(3)给出的状态转移方程，利用第k-1帧图像采样时刻目标相对姿态估计值和目标在相机坐标系下的旋转角速度矢量估计值计算第k帧图像采样时刻相对姿态预测值和旋转角速度矢量预测值

其中，上标∧表示估计值，上标∧和下标“k/k-1”构成预测值；

然后根据第k帧图像采样时刻相对姿态预测值目标上的第j个SURF特征点在目标本体坐标系下的三维坐标和第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标的实际测量值z_j,k，由测量反演法计算第k帧图像采样时刻目标和相机间的相对位置预测值由测量反演法计算第k帧图像采样时刻目标和相机间的相对位置预测值的过程以函数h(·)表示，即

由测量反演法计算第k帧图像采样时刻目标和相机间的相对位置预测值的具体方法为：由式(7)得到的目标上的第j个SURF特征点在目标本体坐标系下的三维坐标和第k帧图像采样时刻目标和相机间的相对位置预测值为未知量，根据式(4)和式(5)计算第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标预测值

其中，表示第k帧图像采样时刻目标上的第j个SURF特征点在相机坐标系下的坐标预测值，分别为的X、Y、Z轴坐标，表示第k帧图像采样时刻目标本体坐标系到相机坐标系的变换矩阵预测值，分别为的X、Y轴坐标，根据步骤二得到的z_j,k＝[x_j,k y_j,k]^T以及由式(9)获得的第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标预测值构建第k帧图像采样时刻第j个SURF特征点的投影误差代价函数e_j,k

令e_j,k＝0，将(9)式代入式(10)并化简，得

其中分别对应的每一行向量，A为2M×3的矩阵，B为2M×1向量；如果检测的特征点数量M大于2，则式(11)为超定方程，由最小二乘法求解得到

依据上述对第k帧图像采样时刻目标和相机间的相对位置ρ_k的预测，重新选取第k帧图像采样时刻目标的状态向量s_k＝[α_k ω_k ρ_k]，优化目标状态转移方程为

s_k＝f(s_k-1)+q_k

其中s_k-1表示第k-1帧图像采样时刻目标的状态向量；

步骤三中，采用无迹粒子滤波UPF算法估计相机和目标间的相对位姿的具体过程如下：

在初始采样时刻k＝0，由步骤一中初始化得到的α₀和ρ₀作为状态向量的初始值为初始状态误差协方差矩阵，以一个高斯分布随机产生N个为第i个粒子所对应的相对位姿状态向量的初始值，i＝1,2,…,N，N为粒子的总数；初始化粒子权值对N个粒子加权求和，即求粒子的平均值以作为每个粒子的均值计算每个粒子协方差矩阵其中为第i个粒子所对应的相对位姿状态向量的初始值，为第i个粒子的均值；

对后续时刻的具体处理步骤如下：

步骤S1:利用无迹卡尔曼滤波UKF生成粒子滤波PF的重要性概率密度函数，更新第k帧图像采样时刻第i个粒子的均值和第k帧图像采样时刻第i个粒子的状态误差协方差矩阵并根据和生成新的粒子集合步骤S1的具体方法为：

假设第k-1帧图像采样时刻第i个粒子均值为第k-1帧图像采样时刻的第i个粒子状态误差协方差矩阵为粒子权值为状态向量维数为n，总粒子数为N，对每个粒子，以为基准，对称分布采样2n+1个样本点其中u＝0,...,2n表示样本点索引：

并计算第i个粒子的第u个样本点权值

其中λ为微调参数，控制样本点到均值的距离；由组成的样本点集合也被称为Sigma点集合；

由式(12)目标状态转移方程计算样本点预测值

并计算样本点加权均值的一步预测和状态误差协方差矩阵的一步预测

令分别表示第k帧图像采样时刻第i个粒子的第u个样本点预测值中的相对姿态和相对位置分量，根据式(6)测量方程计算第k帧图像采样时刻目标上所有M个SURF特征点图像物理坐标预测测量值所构成的矩阵表示第k帧图像采样第i个粒子的第u个样本点所描述的相对位姿状态下，目标上的第j个SURF特征点图像物理坐标预测测量值；得到：

进一步形成的加权均值和其自协方差矩阵以及第i个粒子的第u个样本点预测值和的互协方差矩阵如下式所示：

根据上式和计算无迹卡尔曼滤波UKF增益更新第k帧图像采样时刻第i个粒子的均值和第k帧图像采样时刻第i个粒子的状态误差协方差矩阵

由和生成粒子滤波PF的重要性概率密度函数采样第k帧图像采样时刻第i个粒子其中，N(a,b)表示均值为a，协方差矩阵为b的高斯分布；更新第k帧图像采样时刻第i个粒子权值

其中似然函数服从均值为协方差矩阵为N_k的高斯分布，即表示第k帧图像采样时刻第i个粒子所描述的相对位姿状态下目标上所有M个SURF特征点图像物理坐标的预测测量值，状态转移概率服从均值为协方差矩阵为Q_k的高斯分布，即表示第k帧图像采样时刻第i个粒子的预测值；归一化第k帧图像采样时刻第i个粒子权值

步骤S2:采用系统重采样算法进行粒子重采样：重采样前第k帧图像采样时刻第i个粒子重采样后更新第k帧图像采样时刻第i个粒子的均值和协方差矩阵为和即重采样后更新第k帧图像采样时刻第i个粒子平均第k帧图像采样时刻第i个粒子权值

步骤S3:重采样后，对第k帧图像采样时刻第i个粒子进行加权求和，得到第k帧图像采样时刻状态向量估计值从而即得到相机和目标间的相对姿态估计值和相对位置估计值