一种基于惯性和地磁传感器的目标姿态跟踪方法
【技术领域】
[0001] 本发明涉及智能传感与人机交互技术领域，尤其涉及一种基于惯性和地磁传感器 的目标姿态跟踪方法。
【背景技术】
[0002] 基于姿态跟踪的增强现实技术将虚拟的三维物体、视频、文字、图片等计算机生成 的信息实时叠加显示到真实场景中，通过姿态跟踪实现自然的人机交互，在医疗卫生、军事 仿真、工业维修、教育娱乐等行业具有广泛的应用前景。增强现实中的姿态跟踪的目的在于 识别运动目标并获得其姿态变化信息，所获取的姿态信息通常被应用在如下五方面：
[0003] 1)视野控制，利用姿态信息控制虚拟摄像机的位置和方向；
[0004] 2)导航，协助用户在虚拟环境中进行移动；
[0005] 3)虚实交互，通过跟踪用户的肢体，实现抓取、移动等交互功能；
[0006] 4)仪器跟踪，跟踪系统中的仪器设备，如手术工具、维修工具等；
[0007] 5)姿态传递，将用户的姿态信息传递给随动设备，如手术机器人或电影中的虚拟 角色。
[0008] 常用于姿态跟踪的惯性传感器件包括惯性陀螺仪和加速计等。在测量中采用捷联 式惯性导航原理目标姿态，其中惯性陀螺主要用于测量三轴旋转角速度或角加速度，并利 用增量积分法获得跟踪器的方向信息，但会产生随时间增加的漂移误差；加速计则主要用 于测量三个敏感轴上的重力加速度分量，再利用余弦算法获得跟踪器的俯仰角和横滚角并 克服陀螺仪带来的这两个姿态角上的漂移误差。惯性跟踪器具有体积小、重量轻、无遮挡问 题、工作范围不受限制以及刷新率高等优点，同时利用磁场传感器可以得到航向角的特点 可以实现对目标的三维姿态跟踪。
[0009] 利用惯性传感器进行人体姿态跟踪的方法提出于1999年（参见Jihong Lee, Insoo Ha. Sensor fusion and calibration for motion captures using accelerometers. Robotics and Automation, Volume 3, Page 1954 - 1959. 1999) 〇 该方法 利用地球重力在加速度计三个敏感轴上的分量计算目标的俯仰角和横滚角，可以得到目标 在两个自由度上的姿态信息，缺点是仅通过重力分量无法得到完整的三自由度姿态信息并 且加速度传感器数据不适用于可提高系统精度的卡尔曼滤波器。
[0010] 利用陀螺仪、加速度计和磁传感器的姿态跟踪方法如2007年，Rehbinder, H等 (参见 Rehbinder, H.，Xiaoming Hu. Drift-free attitude estimation for accelerated rigid bodies. Robotics and Automation, Volume 4, Page 4244 - 4249.2001)提出使用多 传感器融合方法，通过结合角速度、重力加速度和磁场分量并利用卡尔曼滤波器进行误差 校正，实现了对目标三维姿态的实时跟踪，但是该方法没能解决线性加速度对目标姿态跟 踪的影响。
【发明内容】
toon] 有鉴于此，本发明提供了一种基于惯性和地磁传感器的目标姿态跟踪方法，无需 复杂的跟踪设备和特定的跟踪环境，使用基于惯性和地磁传感器即可完成对目标的实时三 维姿态跟踪，且跟踪结果具有精度和刷新率高的特点。
[0012] 本发明的一种基于惯性和地磁传感器的目标姿态跟踪方法，采用陀螺仪、加速度 计以及地磁传感器分别采集目标当前姿态所对应的角速度、加速度和磁强度数据在三个敏 感轴上的分量，并采用卡尔曼滤波器对上述三个传感器数据进行融合并计算目标的姿态信 息，具体方法如下：
[0013] 步骤1、通过卡尔曼滤波器对目标姿态信息进行时间更新，具体为：
[0014] S10、在时间更新方程中对目标的状态矢量尤进行估计：
【主权项】
1. 一种基于惯性和地磁传感器的目标姿态跟踪方法，其特征在于，采用陀螺仪、加速度 计以及地磁传感器分别采集目标当前姿态所对应的角速度、加速度和磁强度数据在三个敏 感轴上的分量，并采用卡尔曼滤波器对上述三个传感器数据进行融合并计算目标的姿态信 息，具体方法如下： 步骤1、通过卡尔曼滤波器对目标姿态信息进行时间更新，具体为： 510、 在时间更新方程中对目标的状态矢量足进行估计： 其中，A为所述陀螺仪测得的目标在三个敏感轴上角速度测量值构成的滤波器增益矩 阵；尤为当前时刻目标状态矢量估计值，尤_,为卡尔曼滤波器上一时刻输出的目标状态矢 量； 511、 对卡尔曼滤波器的传递函数的估计值4进行更新： P； = APk_xAT + Q 其中，Q值为系统噪声协方差矩阵;At为增益矩阵A的转置矩阵；PlrtS卡尔曼滤波器 上一时刻输出的传递函数； 步骤2、通过卡尔曼滤波器对目标姿态信息进行测量更新，具体为： 520、 基于步骤1中更新得到的传递函数的估计值4 ,获得卡尔曼滤波器的测量置信参 数Kk: Kk =Pk Ht (HPk Ht +R)-1 其中，H为测量模型与估计模型间转换矩阵：
Iiq1 q2 q3 q4]表示组成目标的状态矢量估计值足的旋转四元数，即尤七f: ft f4f ;R 为测量误差模型：A =g2;其中ACCX、ACC# ACC z分别为加速度传感器 测得的目标在三个敏感轴上的加速度分量，g为重力加速度； 521、 对目标的状态矢量的测量结果足进行更新： Xt-Xl+Kk-iZk-HXp
表示目标的姿态，其中，Θ =arcsin(ACCx)表示目标的俯仰角，
表示目标的横滚角，
表示目标的航向角，A = MAGxCos ( Θ ) +MAGySin ( Θ ) +MAGzCos (Θ ) sin ( γ )，B = MGyCos ( γ ) +MGzSin ( γ )，MAGX、MAGjP MG 2分别表示磁场传感器获得 的目标所在位置的地球磁场在体坐标系的三个敏感轴上的分量； 522、 对传递函数的测量结果Pk进行更新： Pk=(I-KkIl)Pk 步骤3、目标状态矢量的测量结果足即为当前目标姿态的跟踪结果，将当前时刻测量 结果尤和传递函数的测量结果Pk输出并作为下一时刻滤波器的输入结果，返回步骤1，进 行下一时刻的目标姿态跟踪。
2.如权利要求1所述的一种基于惯性和地磁传感器的目标姿态跟踪方法，其特征在 于，所述步骤1中系统噪声协方差矩阵Q取值为〇. 01。
【专利摘要】本发明公开了一种基于惯性和地磁传感器的目标姿态跟踪方法，无需任何参考标志物和特定跟踪环境即可实现姿态跟踪，方法简单易行；采用陀螺仪、加速度计以及地磁传感器分别采集目标当前姿态所对应的角速度、加速度和磁强度数据在三个敏感轴上的分量，利用加速度计和磁传感器校正陀螺仪的姿态跟踪结果，消除了漂移误差，从而提高跟踪结果的精度；本发明使用高效离线运行的卡尔曼滤波算法，融合了多传感器跟踪结果，实现了实时在线姿态跟踪。
【IPC分类】G01C21-16, G01C21-08
【公开号】CN104764451
【申请号】CN201510196626
【发明人】刘越, 贺长宇, 闫达远, 常军, 翁冬冬
【申请人】北京理工大学
【公开日】2015年7月8日
【申请日】2015年4月23日