一种基于惯性和地磁传感器的目标姿态跟踪方法

摘要

本发明公开了一种基于惯性和地磁传感器的目标姿态跟踪方法，无需任何参考标志物和特定跟踪环境即可实现姿态跟踪，方法简单易行；采用陀螺仪、加速度计以及地磁传感器分别采集目标当前姿态所对应的角速度、加速度和磁强度数据在三个敏感轴上的分量，利用加速度计和磁传感器校正陀螺仪的姿态跟踪结果，消除了漂移误差，从而提高跟踪结果的精度；本发明使用高效离线运行的卡尔曼滤波算法，融合了多传感器跟踪结果，实现了实时在线姿态跟踪。

主权项

一种基于惯性和地磁传感器的目标姿态跟踪方法，其特征在于，采用陀螺仪、加速度计以及地磁传感器分别采集目标当前姿态所对应的角速度、加速度和磁强度数据在三个敏感轴上的分量，并采用卡尔曼滤波器对上述三个传感器数据进行融合并计算目标的姿态信息，具体方法如下：步骤1、通过卡尔曼滤波器对目标姿态信息进行时间更新，具体为：S10、在时间更新方程中对目标的状态矢量进行估计：其中，A为所述陀螺仪测得的目标在三个敏感轴上角速度测量值构成的滤波器增益矩阵；为当前时刻目标状态矢量估计值，为卡尔曼滤波器上一时刻输出的目标状态矢量；S11、对卡尔曼滤波器的传递函数的估计值进行更新：$p_k^{-}={\mathrm{Ap}}_{k-1}{A}^T+Q$其中，Q值为系统噪声协方差矩阵；A^T为增益矩阵A的转置矩阵；P_k‑1为卡尔曼滤波器上一时刻输出的传递函数；步骤2、通过卡尔曼滤波器对目标姿态信息进行测量更新，具体为：S20、基于步骤1中更新得到的传递函数的估计值获得卡尔曼滤波器的测量置信参数K_k：$K_k=P_k^{-}{H}^T{({\mathrm{HR}}_k^{-}{H}^T+R)}^{-1}$其中，H为测量模型与估计模型间转换矩阵：[q₁ q₂ q₃ q₄]表示组成目标的状态矢量估计值的旋转四元数，即R为测量误差模型：其中ACC_x、ACC_y和ACC_z分别为加速度传感器测得的目标在三个敏感轴上的加速度分量，g为重力加速度；S21、对目标的状态矢量的测量结果进行更新：$Z_k=\left[\begin{array}{c}\theta \\ \gamma \\ \psi \end{array}\right]$表示目标的姿态，其中，θ＝arcsin(ACC_x)表示目标的俯仰角，表示目标的横滚角，表示目标的航向角，A＝MAG_xcos(θ)+MAG_ysin(θ)+MAG_zcos(θ)sin(γ)，B＝MAG_ycos(γ)+MAG_zsin(γ)，MAG_x、MAG_y和MAG_z分别表示磁场传感器获得的目标所在位置的地球磁场在体坐标系的三个敏感轴上的分量；S22、对传递函数的测量结果P_k进行更新：$P_k=(1-K_{l}H)P_k^{-}$步骤3、目标状态矢量的测量结果即为当前目标姿态的跟踪结果，将当前时刻测量结果和传递函数的测量结果P_k输出并作为下一时刻滤波器的输入结果，返回步骤1，进行下一时刻的目标姿态跟踪。