一种被动式光学运动捕捉方法

本发明涉及运动捕捉，尤其涉及一种被动式光学运动捕捉方法。

背景技术：

1、被动光学式运动捕捉方法是移动机器人进行高精度位姿测量的最有效手段，该方法使用逆反射靶球作为标记点，基于计算机图形学，令相机捕捉标记点的帧间变化从而记录待测物体的运动状况，得到移动机器人的高精度位置和姿态，然而在复杂条件下，被动式光学运动捕捉方法存在局限性，主要为受复杂背景和遮挡条件影响，其位姿测量结果精度下降：在一定自然光光照下，测量场地形成复杂背景，目标粘连及大量反射光点的干扰导致标记点的提取错误和匹配错误；测量过程中靶球会受到静遮挡、互遮挡和自遮挡的遮挡条件影响，使部分标记点无法被相机观测，导致参与位姿估计的标记点数量减少和匹配错误。

2、解决遮挡条件下的移动机器人运动捕捉问题，本质是对于遮挡条件下进行跟踪，现有方法包括：基于有效特征信息、状态估计信息、稳定时空信息的目标跟踪算法。

3、(1)基于有效特征信息的目标跟踪方法。包括光流运动特征法、手工设计的外观特征法及深度特征法等，如使用软图匹配模型，针对遮挡进行姿态重建从而估计缺失的标记位置，但该方法计算复杂，需针对模板进行设计与训练。其中基于深度学习的目标跟踪算法近年来发展迅速，往往能获得更高的跟踪准确度，但精度、速度和鲁棒性等方面仍有待提高。

4、(2)基于状态估计信息的目标跟踪方法。包括卡尔曼滤波、粒子滤波两种经典方法，能够通过预测实现抗遮挡。如使用离线训练对于严重遮挡使用前几帧位移和角度平均值进行估算，使用基于恒速模型的卡尔曼滤波，对每个缺失的标记点三维坐标进行预测并重建，但该方法运动过程建模较为简单，无法应对复杂的六自由度运动；或使用自适应无迹卡尔曼滤波进行分布式融合，该方法使用平面靶标和单目子系统，但所适用的运动范围过小。

5、(3)基于稳定时空信息的目标跟踪方法。在本发明实施的方法中，主要利用时间、空间及时空上下文提升遮挡场景下的跟踪稳定性。然而本方法对于跟踪稳定性效果提升较多，无法保证遮挡情况下的位姿估计精度提升。

6、因此，本领域的技术人员致力于开发一种适用于复杂条件下的被动式光学运动捕捉方法，以解决复杂背景和遮挡条件导致的系统位姿估计精度下降问题。

技术实现思路

1、有鉴于现有技术的上述缺陷，本发明所要解决的技术问题是基于无迹卡尔曼滤波方法抗遮挡精度不足，复杂背景和遮挡条件导致的系统位姿估计精度下降。

2、为实现上述目的，本发明提供了一种被动式光学运动捕捉方法，包括如下步骤：

3、s1、使用动态调整观测噪声的自适应无迹卡尔曼算法滤波；

4、s2、基于固定靶标的特征重建方法以及基于单目epnp算法的特征重建方法，进行特征重建作为观测量进行待测目标的状态估计；

5、s3、基于松耦合方案，使用矩阵加权的无迹卡尔曼滤波形式进行多组双目系统间的信息融合。

6、在本发明的较佳实施方式中，所述s1中自适应无迹卡尔曼滤波算法，包括：

7、系统初始化并生成sigma点；

8、计算每个sigma点对立的权值；

9、计算一步预测的状态均值与协方差；

10、计算预测观测值的均值和方差；

11、更新得到k时刻状态均值与协方差；

12、更新观测噪声rk。

13、在本发明的另一个较佳实施方式中，所述每个sigma点对立的权值为：

14、

15、

16、

17、其中，和为初始sigma点对立的均值权值和协方差权值，wim和wic为第i 个sigma点对立的均值权值和协方差权值，α取0.001，β取2，λ为比例因子。

18、在本发明的另一个较佳实施方式中，所述s2中基于单目epnp算法的特征重建方法，包括：

19、对于待重建的特征点piω，有：

20、

21、其中，为靶球二维像素坐标，为固定靶标在世界坐标系中的变换矩阵：

22、

23、其中，为当前靶标坐标系相对于世界坐标系的平移向量，为前靶标坐标系相对于世界坐标系的旋转矩阵；

24、所述通过未受遮挡影响的逆反射靶球求出，其余缺失点基于固定的靶标坐标系进行特征重建，从而补全特征信息，作为观测量进行状态估问题的更新。

25、在本发明的另一个较佳实施方式中，所述计算如下：

26、对于每个靶球的初始三维世界坐标p0，有初始三维世界坐标质心p0：

27、

28、定义该时刻，所述p0的质心为：

29、

30、经由双目深度估计，得到某时刻至少三个靶球的三维世界坐标pb：

31、

32、其中n为≥3的正整数，同时定义该时刻，所述pb的质心为：

33、

34、由于存在遮挡情况，当前只有n个靶球的三维世界坐标已知，且n≥3，则第i个靶球所对应第i个点的去质心坐标分别为：

35、

36、

37、其中，为某时刻第i个点的去质心坐标，为第i个点的去质心初始三维世界坐标，p0为初始三维世界坐标质心，为第i个点的初始三维世界坐标，pb为某时刻靶球的质心，为某时刻第i个靶球的坐标。

38、根据下式优化问题，计算靶标整体相对于初始状态的旋转矩阵r*：

39、

40、使用svd分解解出最优r，即得所述为：

41、

42、

43、其中，u和v由w的svd分解得：

44、w＝uσvt

45、σ为奇异值组成的对角矩阵；

46、求得旋转矩阵后，可得所述为：

47、

48、则在遮挡部分靶球的特征重建中，可以使用靶标上可观测靶球及整体的固定位置关系进行特征重建。

49、在本发明的另一个较佳实施方式中，所述s2中基于单目epnp算法的特征重建方法，利用单目相机中观测到的靶球二维像素坐标pib和固定的靶标坐标系，求解出固定靶标在相机坐标系的变换矩阵相机坐标系相对于世界坐标系的变换矩阵通过双目外参标定求出，则经过pnp方法重建的靶球三维世界坐标piw'为：

50、

51、通过已知的未遮挡靶球像素坐标和各靶球在靶球坐标系的相对位置关系，计算求解固定靶标在相机坐标系的变换矩阵从而进行缺失靶球的特征重建。

52、在本发明的另一个较佳实施方式中，所述基于单目epnp算法将已知特征点的三维坐标使用四个虚拟控制点的加权和进行表示，令已知特征点的三维世界坐标为：

53、

54、其中，aij为权重，则在相机坐标系下同样可以用虚拟控制点的加权和表示已知特征点的三维坐标：

55、

56、

57、根据相机的投影模型，已知特征点观测到的第i个像素坐标与虚拟控制点存在如下关系：

58、

59、可转化为两个线性方程：

60、

61、

62、将全部n个特征点的2n个线性方程联立可得方程组：

63、mx＝0

64、其中m为2n×12的矩阵，是一个虚拟控制点在相机坐标系的坐标，为12维向量，且有：

65、

66、其中，vi为矩阵m的右奇异向量，n为矩阵m的n个零特征值计算mtm的特征向量得到vi，且由于虚拟控制点的空间位置不变，计算n取1,2,3,4的零空间误差，选择最小的维数对应的βi，从而求得x，即虚拟控制点的相机坐标系坐标随后计算的重心和矩阵a：

67、

68、

69、计算的重心和矩阵b：

70、

71、

72、令h＝bta，计算h的svd分解有：

73、h＝uσvt

74、由此计算位姿变换的旋转分量r：

75、r＝uvt

76、利用r计算位姿中的平移分量t：

77、

78、在本发明的另一个较佳实施方式中，所述s3中多组双目系统间的信息融合方法，包括：

79、选取多目相机中一组协方差矩阵最小的双目子系统确定系统状态方程；

80、将k时刻的m个双目子系统使用自适应无迹卡尔曼滤波计算状态估计均值xk和协方差矩阵p，将协方差矩阵按迹的大小进行排列，则协方差矩阵迹最小的系统定义为基准子系统；

81、在当前时刻将全部子系统计算自适应无迹卡尔曼滤波，使用预测的协方差矩阵计算基准子系统后，将其他子系统预测结果与基准子系统融合，从而实现多目相机的松耦合。

82、在本发明的另一个较佳实施方式中，用所述基准子系统进行如下计算：

83、选取2n+1个sigma点：

84、

85、其中，表示k-1时刻基准子系统的状态估计值，为其对应的状态误差协方差矩阵；

86、将sigma点通过状态转移矩阵变换：

87、

88、求取基准子系统的状态预测值：

89、

90、计算获取各个sigma点对应的状态预测误差：

91、

92、根据sigma点对应权值进行加权，计算基准子系统的预测误差协方差：

93、

94、将其余双目子系统的状态估计及协方差进行加权，根据矩阵加权融合结果计算多目松耦合系统协方差：

95、

96、其中，i表示局部子系统的序号，m为多视觉传感器位姿估计系统中局部相机子系统的总个数，pi为第i个子系统的状态误差协方差，ps为松耦合系统总的状态预测误差协方差；

97、计算松耦合后系统的状态估计均值：

98、

99、本发明还提出了一种被动式光学运动捕捉系统，采用上述的被动式光学运动捕捉方法。

100、本发明的有益效果：

101、1、本发明针对复杂背景下目标提取时二维图像上外点剔除不彻底的问题，提出了三维空间下利用先验体积约束的运动捕捉方法，利用双目相机和逆反射靶标，实现了复杂背景下高精度的六自由度位姿测量。

102、2、本发明针对现有基于无迹卡尔曼滤波方法抗遮挡精度不足的问题，提出了基于自适应无迹卡尔曼滤波与靶标特征重建的运动捕捉方法，该方法对于多目系统能够基于松耦合进行信息融合，实现了遮挡条件下完整且连续的高精度六自由度位姿测量。

103、3、本发明首次建立了针对少/弱纹理、运动模糊、光线变化、复杂动态等挑战性因素的数据集，该数据集提供了基于运动捕捉的毫米量级高精度位姿真值。

104、以下将结合附图对本发明的构思、具体结构及产生的技术效果作进一步说明，以充分地了解本发明的目的、特征和效果。