[发明专利]基于多视觉传感器分布式信息融合的遮挡情况下目标位姿估计方法

摘要

一种基于多视觉传感器分布式信息融合的遮挡情况下目标位姿估计方法，判断所有视觉传感器局部子系统中是否出现严重遮挡情况，若出现则放弃对该局部子系统进行计算，并等待剩余子系统融合结束，反之，则判断是否出现部分遮挡情况，若没有，利用无迹卡尔曼滤波器求出局部子系统的状态信息，进而筛选出基础状态量；若出现遮挡，利用得到的基础状态量和先验知识估计出现部分遮挡的子系统中缺失的特征点像素坐标，然后再利用无迹卡尔曼滤波器得到相应局部子系统的状态信息；利用分布式融合方法得到最终的融合结果，最后将出现严重遮挡局部子系统初始化。本发明能够在出现遮挡的情况下有效地跟踪运动刚体的位姿，具有很强的鲁棒性和有效性。

主权项

1.一种基于多视觉传感器分布式信息融合的遮挡情况下目标位姿估计方法，其特征在于：所述的位姿估计方法包括以下步骤：(1)判断所有视觉传感器局部子系统中是否出现严重遮挡情况，若没有出现严重遮挡，执行1.1)；若出现严重遮挡，执行1.2)；1.1)判断是否出现部分遮挡情况，若无部分遮挡情况，执行1.1.1)；若出现部分遮挡情况，执行1.1.2)；1.1.1)利用无迹卡尔曼滤波器(Unscented Kalman Filter，UKF)求出局部子系统的状态估计值和误差协方差矩阵，进一步计算各个局部估计误差协方差的迹，将最小的协方差迹对应的状态估计值选为基础状态量xs：其中，m表示无遮挡的局部子系统个数，P_i和trace(P_i)分别表示第i个局部子系统误差协方差矩阵和对应的迹，s是局部子系统对应的序号，即第s个局部子系统的误差协方差的迹为最小的，基础状态量x_s包含了位置向量及姿态向量其中和分别为第s个局部子系统中物体坐标系相对于世界坐标系的位置和姿态，b表示物体坐标系，ω为世界坐标系，x、y、z分别表示x、y、z三个轴向的坐标值，α、β、γ分别表示绕x、y、z三个轴旋转的角度；1.1.2)针对部分遮挡的情况，执行步骤1.1.2.1至步骤1.1.2.5；步骤1.1.2.1利用得到的基础状态量x_s计算从物体坐标系到世界坐标系的旋转矩阵步骤1.1.2.2利用位置向量和求得的旋转矩阵求出特征点在世界坐标系下的坐标l为特征点的序号：其中为特征点在物体坐标系下的坐标；步骤1.1.2.3根据相机与世界坐标系之间的转换关系，利用求得的坐标计算出特征点在相应相机坐标系中的坐标其中cjRω和cjtω分别为第j个相机坐标系与世界坐标系的旋转矩阵与平移矩阵，c表示相机坐标系；步骤1.1.2.4将步骤1.1.2.3中求得的第l个特征点在相机坐标系下的坐标，反投影到第j个相机对应的图像存储坐标系中，得到该特征点在相应像素坐标系下的坐标(ul，vl)，从而补全因遮挡而缺失的特征点像素坐标：其中，l、u_l、v_l分别表示第j个出现遮挡的子系统没有观测到的特征点序号以及该特征点在图像存储坐标中对应的水平和竖直方向坐标值，分别表示第j个出现遮挡的子系统没有观测到的第l个特征点在相机坐标系下的x、y、z方向的坐标值，f_x、f_y分别表示图像存储坐标系中的x、y方向的焦距，u₀、v₀分别表示图像中心在图像存储坐标系中的横纵坐标；步骤1.1.2.5补全缺失的观测信息后，利用UKF得到出现部分遮挡的局部子系统的状态信息；1.2)放弃对该局部子系统进行计算，并利用矩阵加权分布式融合算法对剩余子系统的估计结果进行融合，得到k时刻融合后的估计值，将融合结果反馈给该局部子系统对应的局部估计器，从而使该局部子系统的估计结果重新初始化：xj(k|k)＝x(d)(k|k)Pj(k|k)＝P(d)(k|k)其中，k表示离散时刻，x(d)(k|k)和P(d)(k|k)分别表示k时刻通过分布式融合方法得到的状态的估计值和误差协方差矩阵，xj(k|k)和Pj(k|k)分别表示k时刻该出现严重遮挡的局部子系统对应的状态估计量和状态误差协方差。