CN115914841B 一种被动式光学运动捕捉方法 （上海交通大学）

本发明公开了一种被动式光学运动捕捉方调整观测噪声的自适应无迹卡尔曼算法滤波；景下目标提取时二维图像上外点剔除不彻底的2S2、基于固定靶标的特征重建方法以及基于单目ES3、基于松耦合方案，使用矩阵加权的无迹所述通过未受遮挡影响的逆反射靶球求出，其余缺失3.如权利要求2所述的被动式光学运动捕捉方法，其3则在遮挡部分靶球的特征重建中，可以使用靶标算法的特征重建方法，利用单目相机中观测到的靶球二维像素坐标Pib和固定的靶标坐标系，求解出固定靶标在相机坐标系的变换矩阵相机坐标系相对于世界坐标系的变换矩通过已知的未遮挡靶球像素坐标和各靶球在靶球坐标系的相对位置关4定靶标在相机坐标系的变换矩阵T,从而进行缺失靶球的特征重建。将已知特征点的三维坐标p"使用四个虚拟控制点的加权和进行表示，令已知特征点的ij为权重，则在相机坐标系下同样可以用虚拟控制点的加权和表示已知特征存在如下关系：Mx＝05TTTTt=pf-RP"选取多目相机中一组协方差矩阵最小的双目子系统确定将k时刻的M个双目子系统使用自适应无迹卡尔曼滤波计算状态估计均值xk和协方差矩在当前时刻将全部子系统计算自适应无迹卡尔曼滤波，使用预6将其余双目子系统的状态估计及协方差进行加权，根据矩阵加权其中，i表示局部子系统的序号，M为多视觉传感器位姿估9.一种被动式光学运动捕捉系统，其特征在于，采用7[0003]解决遮挡条件下的移动机器人运动捕捉问题，本质是对[0005](2)基于状态估计信息的目标跟踪方法。包括卡尔曼滤波、粒子滤波两种经典方8[0024]其中，Ws"和wf为初始Sigma点对立的均值权值和协方差权值，W[0025]在本发明的另一个较佳实施方式中，所述S2中基于单目EPnP算法的特征重建方9qf=pb-p[0043]q'=p'-pq:为某时刻第i个点的去质心坐标，为第i个点的去质心初始三维世界坐[0056]在本发明的另一个较佳实施方式中，所述S2中基于单目EPnP算法的特征重建方在相机坐标系的变换矩阵相机坐标系相对于世界坐标系的变换矩阵通过双目外参解固定靶标在相机坐标系的变换矩阵从而进行缺失靶[0064]根据相机的投影模型，已知特征点观测到的第i个像素坐标与虚拟控制点cf存在如下关系：[0071]其中M为2n×12的矩阵，是一个虚拟控制点在相机坐标系的坐[0085]在本发明的另一个较佳实施方式中，所述S3中多组双目[0087]将k时刻的M个双目子系统使用自适应无迹卡尔曼滤波计算状态估计均值xk和协i为第i个子系统的状态误差协方差，Ps为松耦合系统总的状态预测误差协方[0115]如图1所示，本发明实施例提供一种适用于复杂条件下的被动式光学运动捕捉方角度旋转运动时，其上粘贴的靶球可能由待测机器人本身或场景内其他物体形成前景遮[0122]根据待测目标与遮挡物间关系可划分为三类：待测目标被环境中的静止物体遮自适应无迹卡尔曼滤波状态估计算法；随后评估遮挡情况下双目子系统的标记点缺失情k计算如下式所示：k[0151]扩展卡尔曼滤波(ExtendedKalmanFilter,EKF)在非线性系统的状态估计问题ux=x-+k(16)[0172](2)需要针对非线性的状态转移函数和观测函数计算Jacobian矩阵，对于状态量以及复杂的Jacobian矩阵计算，本实施例使用无迹卡尔曼滤波(UnscentedKalman[0186]将xi直接带入非线性函数f(o得到yi的分布：[0188]由于状态估计问题重点关注状态量的均值与协方差，因此使用下式得到y的均值[0196]其中，xk为六维状态向量涵盖物体在世界坐标系下的位置xp角速度物体的加速度和角加速度Ak为h(x⃞)=T"-"(27)[0206]其中为利用状态量中物体在世界坐标系下角度计算所得的旋转矩阵,为利[0208]R⃞"=X(a)Y(B)Z(Y)(29)现复杂噪声环境的自适应状态估计。然而某些情况中Sage_Husa卡尔曼滤波器无法同时估观测噪声初值R0对于k_1时刻状态值r-[0238]已通过(2)和(3)求得一步预测的状态均值x和协方差P，以及预测的观测均值x和[0267]使用SVD分解解出最优R，即得到当前靶标坐标系相对于世界坐标系的旋转矩阵坐标系相对于世界坐标系的变换矩阵通过双目外参标定求出，则经过PnP方法重建的靶算求解固定靶标在相机坐标系的变换矩阵从而进[0279]作为非迭代的PnP算法，EPnP将已知特征点的三维坐标P"使用四个虚拟控制点[0281]其中aij为权重。则在相机坐标系下同样可以用虚拟控制点的加权和表示已知[0283]根据相机的投影模型，已知特征点观测到的第i个像素坐标与虚拟控制点cf存在如下关系：[0290]其中M为2n×12的矩阵，是一个虚拟控制点在相机坐标系的坐取靶标坐标系的变换矩阵从而求得缺失的每个特征波计算状态估计均值xk协方差矩阵P将协方差矩阵按迹的大小进行排列，则协方差矩阵迹[0331](2)用同样数量的未遮挡靶球，基于固定靶标的特征重建精度高于基于单目EPnP球组成的靶标遮挡其中右侧两个靶球，则此时未遮挡靶标比例为71.4由于71.4％>效范围内共有20台千万级像素的Prime41相机，能够实现定