一种基于惯性和光学跟踪系统的混合位姿跟踪方法
【技术领域】
[0001] 本发明涉及智能传感与人机交互领域，尤其涉及一种基于惯性和光学跟踪系统的 混合位姿跟踪方法。
【背景技术】
[0002] 由于惯性跟踪系统根据陀螺仪的角速度、加速度计的加速度以及磁传感器的磁强 度计算目标的姿态角，具有刷新频率较高以及测量延迟较低的优点且跟踪时不依赖于外部 标志点。将光学与惯性跟踪系统进行融合能够获得实时性更好、精度更高的跟踪结果，在出 现遮挡问题时可以在较短时间继续进行位置和姿态跟踪。
[0003] 国内外研宄者对惯性-光学混合跟踪方法进行了广泛研宄。Young等人于2006年 提出了混合式跟踪系统，其中包含了一个由6台摄像机构成的Vicon光学跟踪系统和一个 固定在目标上的装配了反光标志点的惯性跟踪模块（Inertial Measurement Unit, IMU)， 在保持光学跟踪系统的高精度的同时将刷新频率提高到IOOHz (参见Young M C，Young S S,Sang K. Pose estimation from landmark-based vision and inertial sensors. In:Proceedings of the 2006International Joint Conference on SICE-ICASE. Bexc 〇, Busan, Korea: IEEE, 2006, 1668-1671.)。N. Parnian 于 2008 年提出了用于跟踪笔状目 标的混合跟踪系统，安装了 IMU的目标的尖部可以被四个依弧线排列的摄像机所识别， 混合跟踪系统提高了光学跟踪系统的刷新频率和位置跟踪精度（参见Maidi M, Ababsa F, Mallem M. Vision-inertial tracking system for robust fiducials registration in augmented reality. In:Proceedings of the 2009IEEE Symposium on Computational Intelligence for Multimedia Signal and Vision Processing. Nashville, TN, USA:IE EE, 2009. 83-90)。Maidi于2009年提出了一个由两个摄像机和三个标志物组成的光学跟 踪系统（刷新率为55Hz)以及一个跟标志物绑定的惯性跟踪模块（刷新率为500Hz)组成 的混合跟踪系统，实验结果表明该系统可以实现在短时间光学遮挡条件下的姿态跟踪（参 见 Waechter C, Huber M, Keitler P, Schlegel M, Pustka D, Klinker G. A multi-sensor platform for wide-area tracking. In:Proceedings of the 9th IEEE International Symposium on Mixed and Augmented Reality. Seoul, Korea: IEEE, 2010. 275-276.) 〇 上述 各方法通过融合惯性和光学跟踪方法有效提高了跟踪系统的刷新率并实现了在出现遮挡 时进行短时间继续跟踪，但是在跟踪目标位置时会产生随时间累计的漂移误差且其自身会 产生偏置误差、线性加速度误差等，上述方法并不能够实现在长时间遮挡情况下的准确跟 足示。
【发明内容】
[0004] 有鉴于此，本发明提供了一种基于惯性和光学跟踪系统的混合位姿跟踪方法，可 以解决在标志点被部分遮挡时对目标的准确位置和姿态跟踪，同时利用光学跟踪结果修正 惯性传感器数据，消除了偏置误差。
[0005] 为了解决上述技术问题，本发明是这样实现的：
[0006] 本发明的一种基于惯性和光学跟踪系统的混合位姿跟踪方法，包括：
[0007] 1)、在光学跟踪系统中的标志物上的标志点未被遮挡时，利用光学跟踪系统捕获 当前时刻目标的位置和姿态信息，作为位姿跟踪的结果；
[0008] 2)、在所述标志物上的部分标志点被遮挡，光学跟踪系统只能获得标志点的位置 信息但无法获得目标的姿态信息时，执行如下步骤：
[0009] S21、当前时刻，采用光学跟踪系统得到所述标志点的三维空间坐标Xn、Y# Z n;
[0010] S22、采用加速度计、磁传感器和陀螺仪组成的惯性跟踪系统获得所述标志物当前 时刻的角速度、加速度和磁场的测量值，进而采用卡尔曼滤波器得到表征标志物姿态信息 的俯仰角、横滚角和航向角，以及当前时刻标志物的姿态旋转矩阵R n;
[0011] S23、确定标志物上原点与所述标志点的三维空间位移：Λ χη、Λ Λ Z n;
[0012] S24、基于如下公式得到当前时刻所述原点的三维空间坐标Xp Ytl和Z Q:
[0013]
【主权项】
1. 一种基于惯性和光学跟踪系统的混合位姿跟踪方法，其特征在于，包括： 1) 、在光学跟踪系统中的标志物上的标志点未被遮挡时，利用光学跟踪系统捕获当前 时刻目标的位置和姿态信息，作为位姿跟踪的结果； 2) 、在所述标志物上的部分标志点被遮挡，光学跟踪系统只能获得标志点的位置信息 但无法获得目标的姿态信息时，执行如下步骤： 521、 当前时刻，采用光学跟踪系统得到所述标志点的三维空间坐标Xn、Z n; 522、 采用加速度计、磁传感器和陀螺仪组成的惯性跟踪系统获得所述标志物当前时刻 的角速度、加速度和磁场的测量值，进而采用卡尔曼滤波器得到表征标志物姿态信息的俯 仰角、横滚角和航向角，以及当前时刻标志物的姿态旋转矩阵R n; 523、 确定标志物上原点与所述标志点的三维空间位移：Λ Xn、Λ Zn; 524、 基于如下公式得到当前时刻所述原点的三维空间坐标Ytl和Z μ
525、 根据目标与所述原点既定的位置关系，得到目标的位置信息； 如此，将S22获得的当前时刻的姿态信息和S25获得的当前时刻目标的位置信息，作为 对目标位姿跟踪的结果。
2. 如权利要求1所述的一种基于惯性和光学跟踪系统的混合位姿跟踪方法，其特征在 于： 在1)中，光学跟踪系统进行位姿跟踪的同时，采用陀螺仪、加速度计和磁传感器分别 对目标的角速度、加速度和磁场进行测量；然后根据光学跟踪系统获得的姿态信息，使用余 弦算法计算出目标当前位姿对应的理想的角速度、加速度和磁场，并分别与陀螺仪、加速度 计以及磁传感器的测量值进行比对，获得陀螺仪、加速度计以及磁传感器的偏置误差； 在S22中，在采用惯性跟踪系统获得标志物的角速度、加速度和磁场的测量值后，采用 1)中的偏置误差分别对该三个测量值进行补偿，再根据补偿后的角速度、加速度和磁场，得 到S22中当前时刻目标的姿态信息以及标志物的姿态旋转矩阵R n。
3. 如权利要求1或2所述的一种基于惯性和光学跟踪系统的混合位姿跟踪方法，其特 征在于，所述S22中，采用卡尔曼滤波器获得当前时刻目标的姿态信息的方法为： 5221、 在时间更新方程中对目标的状态矢量尤进行估计： = 其中，A为所述陀螺仪测得的目标在三个敏感轴上角速度测量值构成的滤波器增益矩 阵；尤为当前时刻目标状态矢量估计值，足.,为卡尔曼滤波器上一时刻输出的目标状态矢 量； 5222、 对卡尔曼滤波器的传递函数的估计值进行更新： P, =AP, tAs +Q 其中，Q值为系统噪声协方差矩阵;At为增益矩阵A的转置矩阵；PlrtS卡尔曼滤波器 上一时刻输出的传递函数； 5223、 通过卡尔曼滤波器对目标姿态信息进行测量更新，具体为： 基于步骤S221中更新得到的传递函数的估计值Pp获得卡尔曼滤波器的测量置信参 数Kk: K1 =1] Ι? (IIPiH' I R1 )-' 其中，H为测量模型与估计模型间转换矩阵：
Iiq1 q2 q3 q4]T表示组成目标的状态矢量估计值足的旋转四元数，即* ft ; Rk= R a+Rb为测量噪声协方差矩阵，由匀速运动噪声协方差R a和突变速运动测量噪声协方 差Rb组成，< =AC/ + 3CC>3CC/ -g2，其中ACCx、ACC，ACC z分别为加速度传感器测得的 目标在三个敏感轴上的加速度分量，g为重力加速度； 5224、 对目标的状态矢量的测量结果文进行更新 Xk=X^ + KSZt-HXp
表示目标的姿态，其中，Θ = arcsin(ACC x)表示目标的俯仰 角，
> 表示目标的横滚角，
表示目标的航向角，A = MAGxCos ( Θ ) +MGySin ( Θ ) +MAGzCos ( Θ ) sin ( γ )，B = MAGyCos ( γ ) +MAGzSin ( γ )，MAGX、MAGy 和MAGz分别表示磁场传感器获得的目标所在位置的地球磁场在体坐标系的三个敏感轴上 的分量； 5225、 对传递函数的测量结果Pk进行更新： P1^=(I-K1JI)P, S225、从所述目标状态矢量的测量结果名^中提取当前时刻的目标姿态信息；将当前 时刻测量结果和传递函数的测量结果Pk输出并作为下一时刻滤波器的输入结果，返回 S221，进行下一时刻的目标姿态跟踪。
4.如权利要求1或2所述的一种基于惯性和光学跟踪系统的混合位姿跟踪方法，其特 征在于，所述步骤1)中，当有两个标志点未被遮挡时，采用S21-S24的方法分别基于各标志 点的位置信息得到所述原点的位置信息并求均值，在S25中，利用该原点位置信息的均值， 得到目标的位置。
【专利摘要】本发明公开了一种基于惯性和光学跟踪系统的混合位姿跟踪方法，通过将惯性跟踪系统和光学跟踪系统进行结合使用，可以解决在标志物被部分遮挡时对目标的准确位置和姿态跟踪；利用光学跟踪结果修正惯性传感器数据，消除了偏置误差，提高了惯性跟踪模块在进行姿态估计时的精度；在利用卡尔曼滤波器与惯性跟踪系统进行姿态跟踪时，通过建立加速度误差模型，在测量噪声协方差矩阵中引入突变速运动测量噪声协方差，当系统发生线性运动时，状态方程的测量更新方程中，对测量值的置信程度也会降低，使得状态矢量中估计值占有更大的置信度，减小了加速度计测量值因突变线性加速度的影响而导致的对状态矢量的影响。
【IPC分类】G01C21-16
【公开号】CN104764452
【申请号】CN201510198694
【发明人】刘越, 贺长宇
【申请人】北京理工大学
【公开日】2015年7月8日
【申请日】2015年4月23日