[发明专利]一种基于惯性和光学跟踪系统的混合位姿跟踪方法

摘要

本发明公开了一种基于惯性和光学跟踪系统的混合位姿跟踪方法，通过将惯性跟踪系统和光学跟踪系统进行结合使用，可以解决在标志物被部分遮挡时对目标的准确位置和姿态跟踪；利用光学跟踪结果修正惯性传感器数据，消除了偏置误差，提高了惯性跟踪模块在进行姿态估计时的精度；在利用卡尔曼滤波器与惯性跟踪系统进行姿态跟踪时，通过建立加速度误差模型，在测量噪声协方差矩阵中引入突变速运动测量噪声协方差，当系统发生线性运动时，状态方程的测量更新方程中，对测量值的置信程度也会降低，使得状态矢量中估计值占有更大的置信度，减小了加速度计测量值因突变线性加速度的影响而导致的对状态矢量的影响。

主权项

一种基于惯性和光学跟踪系统的混合位姿跟踪方法，其特征在于，包括：1)、在光学跟踪系统中的标志物上的标志点未被遮挡时，利用光学跟踪系统捕获当前时刻目标的位置和姿态信息，作为位姿跟踪的结果；2)、在所述标志物上的部分标志点被遮挡，光学跟踪系统只能获得标志点的位置信息但无法获得目标的姿态信息时，执行如下步骤：S21、当前时刻，采用光学跟踪系统得到所述标志点的三维空间坐标X_n、Y_n和Z_n；S22、采用加速度计、磁传感器和陀螺仪组成的惯性跟踪系统获得所述标志物当前时刻的角速度、加速度和磁场的测量值，进而采用卡尔曼滤波器得到表征标志物姿态信息的俯仰角、横滚角和航向角，以及当前时刻标志物的姿态旋转矩阵R_n；S23、确定标志物上原点与所述标志点的三维空间位移：ΔX_n、ΔY_n和ΔZ_n；S24、基于如下公式得到当前时刻所述原点的三维空间坐标X₀、Y₀和Z₀：$\left[\begin{array}{c}{X}_0\\ Y_0\\ Z_0\end{array}\right]=\left[\begin{array}{c}{X}_n\\ Y_n\\ Z_n\end{array}\right]-R_n\·\left[\begin{array}{c}{\mathrm{\ΔX}}_n\\ {\mathrm{\ΔY}}_n\\ {\mathrm{\ΔZ}}_n\end{array}\right];$S25、根据目标与所述原点既定的位置关系，得到目标的位置信息；如此，将S22获得的当前时刻的姿态信息和S25获得的当前时刻目标的位置信息，作为对目标位姿跟踪的结果。