欢迎来到知嘟嘟! 联系电话:13095918853 卖家免费入驻,海量在线求购! 卖家免费入驻,海量在线求购!
知嘟嘟
我要发布
联系电话:13095918853
知嘟嘟经纪人
收藏
专利号: 2018106428232
申请人: 浙江工业大学
专利类型:发明专利
专利状态:已下证
专利领域: 测量;测试
更新日期:2023-12-11
缴费截止日期: 暂无
价格&联系人
年费信息
委托购买

摘要:

权利要求书:

1.一种基于多视觉传感器分布式信息融合的遮挡情况下目标位姿估计方法,其特征在于:所述的位姿估计方法包括以下步骤:(1)判断所有视觉传感器局部子系统中是否出现严重遮挡情况,若没有出现严重遮挡,执行1.1);若出现严重遮挡,执行1.2);

1.1)判断是否出现部分遮挡情况,若无部分遮挡情况,执行1.1.1);若出现部分遮挡情况,执行1.1.2);

1.1.1)利用无迹卡尔曼滤波器(Unscented Kalman Filter,UKF)求出局部子系统的状态估计值和误差协方差矩阵,进一步计算各个局部估计误差协方差的迹,将最小的协方差迹对应的状态估计值选为基础状态量xs:其中,m表示无遮挡的局部子系统个数,Pi和trace(Pi)分别表示第i个局部子系统误差协方差矩阵和对应的迹,s是局部子系统对应的序号,即第s个局部子系统的误差协方差的迹为最小的,基础状态量xs包含了位置向量 及姿态向量 其中 和分别为第s个局部子系统中物体坐标系相对于世界坐标系的位置和姿态,b表示物体坐标系,w为世界坐标系,x、y、z分别表示x、y、z三个轴向的坐标值,α、β、γ分别表示绕x、y、z三个轴旋转的角度;

1.1.2)针对部分遮挡的情况,执行步骤1.1.2.1至步骤1.1.2.5;

步骤1.1.2.1利用得到的基础状态量xs计算从物体坐标系到世界坐标系的旋转矩阵步骤1.1.2.2利用位置向量 和求得的旋转矩阵 求出特征点在世界坐标系下的坐标 l为特征点的序号:

其中 为特征点在物体坐标系下的坐标;

步骤1.1.2.3根据相机与世界坐标系之间的转换关系,利用求得的坐标 计算出特征点在相应相机坐标系中的坐标其中cjRw和cjtw分别为第j个相机坐标系与世界坐标系的旋转矩阵与平移矩阵,c表示相机坐标系;

步骤1.1.2.4将步骤1.1.2.3中求得的第l个特征点在相机坐标系下的坐标,反投影到第j个相机对应的图像存储坐标系中,得到该特征点在相应像素坐标系下的坐标(ul,vl),从而补全因遮挡而缺失的特征点像素坐标:其中,l、ul、vl分别表示第j个出现遮挡的子系统没有观测到的特征点序号以及该特征点在图像存储坐标中对应的水平和竖直方向坐标值, 分别表示第j个出现遮挡的子系统没有观测到的第l个特征点在相机坐标系下的x、y、z方向的坐标值,fx、fy分别表示图像存储坐标系中的x、y方向的焦距,u0、υ0分别表示图像中心在图像存储坐标系中的横纵坐标;

步骤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时刻通过分布式融合方法得到j j的状态的估计值和误差协方差矩阵,x (k|k)和P (k|k)分别表示k时刻该出现严重遮挡的局部子系统对应的状态估计量和状态误差协方差;

所述步骤(1)与步骤1.1)中,所述严重遮挡与部分遮挡的定义如下:根据局部子系统观测到的特征点个数ns对遮挡程度进行区分,不同情况下的定义如下:若满足nl<ns<n,则判断该子系统出现部分遮挡,其中nl为区分严重遮挡和部分遮挡的特征点个数阈值,n为特征点总个数;

若满足ns<nl,则判断该子系统出现严重遮挡;

所述步骤1.2)中,所述分布式融合方法包括以下步骤:

1.2.1)对系统进行初始化设置,设定各个局部子系统初始时刻的状态值 误差协方差Pi(0|0),同时设定融合估计的状态值 及P(d)(0|0);

1.2.2)对于第k个时刻,对于每个局部子系统,将采集的图像进行处理,提取特征点的二维图像存储坐标,然后利用UKF计算出各个未被遮挡和出现部分遮挡的局部子系统的状态估计值 及其对应的误差协方差矩阵Pi(k|k);

1.2.3)计算获得第k个时刻矩阵加权融合的融合状态预测值 和状态预测协方差P(d)(k|k-1):

其中 表示k时刻第si个sigma点的状态预测值,Aa是状态转移矩阵, 表示k-1时刻第si个sigma点的状态估计值, 和 分别为第si个sigma点对应的计算均值及其协方差的权值系数,

为第si个sigma点对应的融合状态预测误差,Q(d)为过程噪声协方差矩阵,sigma点的总数为

2sn+1个;

1.2.4)更新k时刻的融合误差协方差矩阵P(d)(k|k)及融合后的状态估计值其中i=1,2,...,L,L表示待融合的局部滤波器个数, 和 分别表示k时刻第i个局部滤波器状态的预测值和估计值,Pi(k|k-1)和Pi(k|k)表示二者对应的误差协方差。