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

权利要求书

1.一种基于惯性和光学跟踪系统的混合位姿跟踪方法，其特征在于，包括：

1)、在光学跟踪系统中的标志物上的标志点未被遮挡时，利用光学跟踪系统捕获当前时刻目标的位置和姿态信息，作为位姿跟踪的结果；

2)、在所述标志物上的部分标志点被遮挡，光学跟踪系统只能获得标志点的位置信息但无法获得目标的姿态信息时，执行如下步骤：

S21、当前时刻，采用光学跟踪系统得到所述标志点的三维空间坐标X_n、Y_n和Z_n；

S22、采用加速度计、磁传感器和陀螺仪组成的惯性跟踪系统获得所述标志物当前时刻的角速度、加速度和磁场的测量值，进而采用卡尔曼滤波器得到表征标志物姿态信息的俯仰角、横滚角和航向角，以及当前时刻标志物的姿态旋转矩阵R_n；

S23、确定标志物上原点与所述标志点的三维空间位移：ΔX_n、ΔY_n和ΔZ_n；

S24、基于如下公式得到当前时刻所述原点的三维空间坐标X₀、Y₀和Z₀：

X0Y0Z0=XnYnZn-Rn·ΔXnΔYnΔZn;]]>

S25、根据目标与所述原点既定的位置关系，得到目标的位置信息；

如此，将S22获得的当前时刻的姿态信息和S25获得的当前时刻目标的位置信息，作为对目标位姿跟踪的结果。

2.如权利要求1所述的一种基于惯性和光学跟踪系统的混合位姿跟踪方法，其特征在于：

在1)中，光学跟踪系统进行位姿跟踪的同时，采用陀螺仪、加速度计和磁传感器分别对目标的角速度、加速度和磁场进行测量；然后根据光学跟踪系统获得的姿态信息，使用余弦算法计算出目标当前位姿对应的理想的角速度、加速度和磁场，并分别与陀螺仪、加速度计以及磁传感器的测量值进行比对，获得陀螺仪、加速度计以及磁传感器的偏置误差；

在S22中，在采用惯性跟踪系统获得标志物的角速度、加速度和磁场的测量值后，采用1)中的偏置误差分别对该三个测量值进行补偿，再根据补偿后的角速度、加速度和磁场，得到S22中当前时刻目标的姿态信息以及标志物的姿态旋转矩阵R_n。