主权项 |
一种基于惯性和光学跟踪系统的混合位姿跟踪方法,其特征在于,包括:1)、在光学跟踪系统中的标志物上的标志点未被遮挡时,利用光学跟踪系统捕获当前时刻目标的位置和姿态信息,作为位姿跟踪的结果;2)、在所述标志物上的部分标志点被遮挡,光学跟踪系统只能获得标志点的位置信息但无法获得目标的姿态信息时,执行如下步骤:S21、当前时刻,采用光学跟踪系统得到所述标志点的三维空间坐标X<sub>n</sub>、Y<sub>n</sub>和Z<sub>n</sub>;S22、采用加速度计、磁传感器和陀螺仪组成的惯性跟踪系统获得所述标志物当前时刻的角速度、加速度和磁场的测量值,进而采用卡尔曼滤波器得到表征标志物姿态信息的俯仰角、横滚角和航向角,以及当前时刻标志物的姿态旋转矩阵R<sub>n</sub>;S23、确定标志物上原点与所述标志点的三维空间位移:ΔX<sub>n</sub>、ΔY<sub>n</sub>和ΔZ<sub>n</sub>;S24、基于如下公式得到当前时刻所述原点的三维空间坐标X<sub>0</sub>、Y<sub>0</sub>和Z<sub>0</sub>:<maths num="0001" id="cmaths0001"><math><![CDATA[<mrow><mfenced open='[' close=']'><mtable><mtr><mtd><msub><mi>X</mi><mn>0</mn></msub></mtd></mtr><mtr><mtd><msub><mi>Y</mi><mn>0</mn></msub></mtd></mtr><mtr><mtd><msub><mi>Z</mi><mn>0</mn></msub></mtd></mtr></mtable></mfenced><mo>=</mo><mfenced open='[' close=']'><mtable><mtr><mtd><msub><mi>X</mi><mi>n</mi></msub></mtd></mtr><mtr><mtd><msub><mi>Y</mi><mi>n</mi></msub></mtd></mtr><mtr><mtd><msub><mi>Z</mi><mi>n</mi></msub></mtd></mtr></mtable></mfenced><mo>-</mo><msub><mi>R</mi><mi>n</mi></msub><mo>·</mo><mfenced open='[' close=']'><mtable><mtr><mtd><msub><mi>ΔX</mi><mi>n</mi></msub></mtd></mtr><mtr><mtd><msub><mi>ΔY</mi><mi>n</mi></msub></mtd></mtr><mtr><mtd><msub><mi>ΔZ</mi><mi>n</mi></msub></mtd></mtr></mtable></mfenced><mo>;</mo></mrow>]]></math><img file="FDA0000704492140000011.GIF" wi="513" he="213" /></maths>S25、根据目标与所述原点既定的位置关系,得到目标的位置信息;如此,将S22获得的当前时刻的姿态信息和S25获得的当前时刻目标的位置信息,作为对目标位姿跟踪的结果。 |