考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法

摘要

一种考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法，用于集成三轴微陀螺仪、三轴微加速度计和三轴磁阻传感器的惯性测量装置，该方法用转置检测到的旋转角速度矢量、加速度矢量和磁场传感器矢量，借助滤波技术实现装置载体的位姿跟踪估计。该方法包括：1)将加速度矢量视为装置载体本身加速度矢量和重力加速度矢量的复合，并对其幅值和归一化的方向矢量分别构建观测方程；2)利用位姿描述四元数、陀螺仪累积误差矢量、装置载体本身加速度矢量构建系统状态向量；3)因观测方程非线性，用无迹卡尔曼滤波技术实现系统的滤波估计过程。同传统忽略载体本身加速度的方法相比，本发明不但能够给出更为准确的估计结果，且拓宽了系统应用范围。

说明书

技术领域

本发明涉及惯性装置的姿态感知技术领域，是一种适用于集成三轴陀螺仪、三轴加速度计和三轴磁场传感器的惯性装置的姿态感知和估计方法，具体是一种考虑加速度补偿和基于无迹卡尔曼滤波(UKF，UnscentedKalman Filter)的惯性位姿跟踪方法。

背景技术

利用机电惯性测量组合技术进行运动载体位姿的跟踪具有非常广阔的前景。惯性跟踪系统的基本原理是在目标初始位置和姿态已知的基础上，依据惯性原理，利用陀螺仪和加速度计等惯性敏感元件测量物体运动的角速度和直线加速度，然后通过积分获得物体的位置和姿态。由于惯性积分存在累积误差效应，通常需要附加磁场传感器等其它感知元件，以便能够在较长运行时间内保证系统的位姿感知精度。

对于集成三轴陀螺仪、三轴加速度计以及三轴磁场传感器的检测装置而言，其位姿跟踪算法通常利用四元数描述装置相应于初始时刻的姿态信息，然后借助于最陡下降法或者卡尔曼(Kalman)滤波技术实现装置姿态的实时跟踪。上述方法在对加速度信息进行处理时，通常假设装置本身的运动加速度幅值远远小于重力加速度，以便能够利用线性技术实现位姿跟踪过程。上述假设所带来的缺陷是姿态感知精度的降低，尤其在装置本身存在较大幅度的运动情况下。

Choukroun D.提出了一种基于四元数的Kalman滤波方法(见Choukroun D.，Bar-Itzhack I.，Oshman Y.，Novel quaternion Kalmanfilter，IEEE Transactions on Aerospace and Electronic Systems，2006，Vol.42，No.1：174-190)。该方法较为详尽地描述了利用Kalman技术实现四元数姿态跟踪的系统数学模型和噪声模型，其突出贡献是系统模型的伪线性化和状态相关噪声的协方差矩阵的更新方法。然而，遗憾的是，该方法仍然没有考虑装置自身加速度对于系统精度的影响，从而导致其精度在某些情况下不能得到保证。

本发明在Choukroun D.所提方法的基础上，充分考虑到装置自身加速度对系统模型的影响，对其带来的非线性问题利用UKF技术对其进行解决，从而提出了一种考虑加速度补偿和基于UKF的惯性位姿跟踪方法。

本发明申请人在申请号为“200810114391.4”的中国专利“基于ZigBee无线单片机的微惯性测量装置”中提供了一种可用于运动载体姿态测量的装置。在该申请中，采用六轴微惯性传感器和三轴磁阻传感器来测量运动载体的姿态，通过基于ZigBee无线单片机对所测得的信号进行姿态解算，并将解算得到的姿态信息以无线方式传送给其他系统或者上位机，该申请在本申请中引入作为参考。

发明内容

本发明的目的是提供一种考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法，适用于集成三轴陀螺仪、三轴加速度计和三轴磁场传感器的惯性装置的惯性位姿跟踪算法，该算法不但能够克服系统误差，而且能够在装置本身存在较大幅度的运动情况下也能保持较高精度。

为了达到上述目的，本发明的技术解决方案是：

一种考虑加速度补偿和基于无迹卡尔曼滤波(UKF，Unscented KalmanFilter)的惯性位姿跟踪方法，适用于以正交方式集成三轴陀螺仪、三轴加速度计和三轴磁场传感器的姿态感知装置；其系统状态向量包含了装置本身的运动加速度，并对其进行滤波估计；包括以下步骤：

【1】保持装置固定不动，当前姿态称为初始姿态；采集三轴加速度传感器和三轴磁阻传感器数据，得到初始姿态下的加速度矢量a_o＝[a_ox，a_oy，a_oz]^T和磁场矢量m_o＝[m_ox，m_oy，m_oz]^T；

【2】在k时刻采集三轴陀螺仪、三轴加速度传感器和三轴磁阻传感器数据，得到装置当前姿态下的旋转角速度矢量ω_t(k)＝[ω_tx(k)，ω_ty(k)，ω_tz(k)]^T、加速度矢量a_t(k)＝[a_tx(k)，a_ty(k)，a_tz(k)]^T和磁场矢量m_t(k)＝[m_tx(k)，m_ty(k)，m_tz(k)]^T；

【3】构建系统状态方程：

定义系统状态向量为：

X(k)＝[q^T(k)，μ^T(k)，a_b^T(k)]^T    (1)

其中：q(k)＝[q₀(k)，q₁(k)，q₂(k)，q₃(k)]^T为描述当前姿态同初始姿态之间相对关系的旋转四元数矢量，μ(k)＝[μ_x(k)，μ_y(k)，μ_z(k)]^T为三轴陀螺仪的累积误差矢量，a_b(k)＝[a_bx(k)，a_by(k)，a_bz(k)]^T为装置自身的运动加速度矢量；

依据上述状态向量的系统状态方程为：

$X\left(k\right)=F(X(k-1),{\omega }_t\left(k\right))+n_x\left(1(k-1)\right)$

$=\left(\begin{array}{ccc}{I}_{4\times 4}+\frac{\mathrm{\Delta t}}{2}M\left({\omega }_t\left(k\right)\right)& -\frac{\mathrm{\Delta t}}{2}E\left(q(k-1)\right)& 0_{4\times 3}\\ 0_{3\times 4}& I_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 4}& 0_{3\times 3}& I_{3\times 3}\end{array}\right)X(k-1)---\left(2\right)$

$+\left(\begin{array}{ccc}-\frac{\mathrm{\Delta t}}{2}E\left(q(k-1)\right)& 0_{4\times 3}& 0_{4\times 3}\\ 0_{3\times 4}& \mathrm{\Delta t}& 0_{3\times 3}\\ 0_{3\times 4}& 0_{3\times 3}& I_{3\times 3}\end{array}\right)n_{\mathrm{Gx}}$

式中Δt为采样周期，n_Gx为噪声矢量，其协方差矩阵为矩阵I为相应阶次的单位矩阵；

【4】构建系统观测方程：

$Z\left(k\right)=\left(\begin{array}{c}{Z}_{\hat{a}}\left(k\right)\\ Z_{\left|a\right|}\left(k\right)\\ Z_{\hat{m}}\left(k\right)\end{array}\right)=\left(\begin{array}{c}\frac{1}{2}\left[A({\hat{a}}_t\left(k\right),{\hat{a}}_{\mathrm{ob}}\left(k\right))\right]q\left(k\right)\\ \left|\right|a_t\left(k\right)\left|\right|-\left|\right|a_{\mathrm{ob}}\left(k\right)\left|\right|\\ \frac{1}{2}\left[A\left({\hat{m}}_t\left(k\right)\right)\right]q\left(k\right)\end{array}\right)+\left(\begin{array}{ccc}\frac{1}{2}E\left(q\left(k\right)\right)& 0_{4\times 1}& 0_{4\times 3}\\ 0_{1\times 3}& 1& 0_{1\times 3}\\ 0_{4\times 3}& 0_{4\times 1}& \frac{1}{2}E\left(q\left(k\right)\right)\end{array}\right)n_{\mathrm{Gz}}$

$=G\left(X\left(k\right)\right)+D\left(X\left(k\right)\right)n_{\mathrm{Gz}}---\left(3\right)$

式中，a_ob(k)＝a_o+a_b(k)，对于任意三维矢量r＝[r_x，r_y，r_z]^T，为其归一化单位方向矢量，||r||为其幅值，矩阵：

$A({\hat{a}}_t\left(k\right),{\hat{a}}_{\mathrm{ob}}\left(k\right))=\left(\begin{array}{cc}0& -\frac{1}{2}{[{\hat{a}}_t\left(k\right)+{\hat{a}}_{\mathrm{ob}}\left(k\right)]}^T\\ \frac{1}{2}[{\hat{a}}_t\left(k\right)+{\hat{a}}_{\mathrm{ob}}\left(k\right)]& \frac{1}{2}{[{\hat{a}}_t\left(k\right)-{\hat{a}}_{\mathrm{ob}}\left(k\right)]}_{\times }\end{array}\right)$

$A\left({\hat{m}}_t\left(k\right)\right)=\left(\begin{array}{cc}0& -\frac{1}{2}{[{\hat{m}}_t\left(k\right)+{\hat{m}}_o]}^T\\ \frac{1}{2}{[{\hat{m}}_t\left(k\right)+{\hat{m}}_o]}^T& \frac{1}{2}{\left[{[{\hat{m}}_t\left(k\right)-{\hat{m}}_o]}^T\right]}_{\times }\end{array}\right)$

其中，[□]_×表示由相应向量定义的反对称矩阵；n_Gz为观测噪声矢量，其协方差矩阵为

【5】系统状态Sigma点采样：根据k-1时刻的系统状态X(k-1/k-1)和协方差矩阵P(k-1/k-1)进行Sigma点采样，得到21个点样本为X_si，i＝1，…，20；

【6】UKF预测：根据方程(2)，对21个Sigma点进行状态预测：

X_spi＝F(X_si，ω_t(k))i＝0，…，21            (4)

利用上述采样预测值确定系统状态向量和协方差矩阵的最终预测值为：

$X(k/k-1)=\underset{i=0}{\overset{21}{\Sigma }}{w}_i{X}_{\mathrm{spi}}---\left(5\right)$

$P(k/k-1)=\underset{i=0}{\overset{21}{\Sigma }}{w}_i{[X_{\mathrm{spi}}-X(k/k-1)][X_{\mathrm{spi}}-X(k/k-1)]}^T+Q_{k-1}---\left(6\right)$

$Q_{k-1}=\left(\begin{array}{ccc}\frac{1}{4}({\sigma }_{\omega 1}^2+{\sigma }_{\omega 2}^2\mathrm{\Delta t})[\mathrm{tr}\left(M_{k-1}\right)I_{4\times 4}-M_{k-1}]& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& {\sigma }_{\omega 3}^2\mathrm{\Delta t}{I}_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& {\sigma }_{a_b}^2{I}_{3\times 3}\end{array}\right)---\left(7\right)$

M_k-1＝q(k-1/k-1)q^T(k-1/k-1)+P_q(k-1/k-1)     (8)

其中P_q(k-1/k-1)为矩阵P(k-1/k-1)中相应于四元数向量的协方差子阵；w_i为相应点样本的权值；

【7】UKF更新：对于Sigma点预测X_spi，令q_i(k/k-1)为由向量X_spi前四个元素得到的归一化四元数，根据观测方程其观测值计算为：

Z_i(k)＝G(X_spi(k/k-1))i＝0，…，21    (9)

而最终观测值计算为：

$Z\left(k\right)=\underset{i=0}{\overset{21}{\Sigma }}{w}_i{Z}_i\left(k\right)---\left(10\right)$

系统状态向量和协方差矩阵的最终更新为：

X(k/k)＝X(k/k-1)-KZ(k)     (11)

P(k/k)＝P(k/k-1)-KP_ZZK^T    (12)

其中：

$K=P_{\mathrm{XZ}}{P}_{\mathrm{ZZ}}^{-1}---\left(13\right)$

$P_{\mathrm{ZZ}}=\underset{i=0}{\overset{21}{\Sigma }}{w}_i[Z_i\left(k\right)-Z\left(k\right)]{[Z_i\left(k\right)-Z\left(k\right)]}^T+\underset{i=0}{\overset{21}{\Sigma }}{w}_i{R}_i---\left(14\right)$

$P_{\mathrm{XZ}}=\underset{i=0}{\overset{21}{\Sigma }}{w}_i[X_{\mathrm{spi}}-X(k/k-1)]{[Z_i\left(k\right)-Z\left(k\right)]}^T---\left(15\right)$

$R_i=\left(\begin{array}{ccc}{D}_{\hat{a}i}& 0_{4\times 1}& 0_{4\times 4}\\ 0_{1\times 4}& {\sigma }_{\left|a\right|}^2& 0_{1\times 4}\\ 0_{4\times 4}& 0_{4\times 1}& D_{\hat{m}i}\end{array}\right)---\left(16\right)$

$D_{\hat{a}i}=\frac{1}{4}{\sigma }_{\hat{a}}^2[\mathrm{tr}\left(M_{\mathrm{ki}}\right)I_{4\times 4}-M_{\mathrm{ki}}-B\left({\hat{a}}_t\left(k\right)\right)M_{\mathrm{ki}}{B}^T\left({\hat{a}}_t\left(k\right)\right)]---\left(17\right)$

$D_{\hat{m}i}=\frac{1}{4}{\sigma }_{\hat{m}}^2[\mathrm{tr}\left(M_{\mathrm{ki}}\right)I_{4\times 4}-M_{\mathrm{ki}}-B\left({\hat{m}}_t\left(k\right)\right)M_{\mathrm{ki}}{B}^T\left({\hat{m}}_t\left(k\right)\right)]---\left(18\right)$

$M_{\mathrm{ki}}=q_i(k/k-1)q_i^T(k/k-1)+P_q(k/k-1)---\left(19\right)$

$B\left(r\right)=\left(\begin{array}{cc}0& -r^T\\ r& {\left[r\right]}_{\times }\end{array}\right)---\left(20\right)$

其中P_q(k/k-1)为矩阵P(k/k-1)中相应于四元数向量的协方差子阵；

【8】将X(k/k)中的四元数向量元素进行归一化处理，并利用四元数表示同欧拉角表示之间的关系将其转换为具有较为直观意义的俯仰角、横滚角和航向角。

所述的考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法，其所述系统状态向量包含了装置本身的运动加速度，从而导致了系统的观测方程非线性，因此采用UKF技术实现对于系统状态的滤波估计。

所述的考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法，其对于加速度矢量和磁场矢量分别依据其单位方向矢量构建观测方程Z_a1(k)和Z_m(k)；为了能够有效地估计出装置本身的运动加速度，观测方程中的Z_a2(k)对加速度矢量进行了幅值限制，从而在单位方向矢量一定的情况下，能够唯一估计出加速度矢量a_b(k)。

所述的考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法，其系统状态方程或系统观测方程，其中的噪声均与系统状态向量相关，对于协方差矩阵的预测和更新均对此进行了处理。

所述的考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法，其由于系统状态包含了装置本身的运动加速度，同以往忽略该项因素相比，能够获取更为精确的姿态估计结果，尤其在转置本身运动幅度较大的情况下；同时，估计得到的装置本身的运动加速度，通过积分实现转置的粗略位置确定。

同传统的忽略载体本身加速度的方法相比，本发明方法能够克服系统误差，给出更为准确的估计结果，在装置本身存在较大幅度的运动情况下也能保持较高精度，同时，估计得到的装置运动加速度可以通过积分实现转置的粗略位置确定，从而拓宽了系统应用范围，可应用于机器人、飞行器、车辆、人体运动等领域的位姿检测。

附图说明：

图1为本发明的考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法的示意框图；

图2为本发明所适用的惯性位姿跟踪装置的结构图；

图3为装置沿X轴做纯旋转运动时，本发明方法和传统Kalman滤波方法的姿态跟踪曲线图；

图4为装置沿Y轴做纯旋转运动时，本发明方法和传统Kalman滤波方法的姿态跟踪曲线图；

图5为装置沿Z轴做纯旋转运动时，本发明方法和传统Kalman滤波方法的姿态跟踪曲线图；

图6为装置以较大加速度沿Y轴做往返平移运动时，本发明方法和传统Kalman滤波方法的姿态跟踪曲线图；

图7为装置以较大加速度沿Y轴做往返平移运动时，本发明方法所提取出的装置本身的自运动加速度。

具体实施方式：

本发明一种考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法采用如图1所示结构，主要包括以下步骤：

1)初始姿态标定(1)：保持装置固定不动，当前姿态称为初始姿态，此时由装置的三个正交轴所确定的坐标系作为世界参考坐标系；采集三轴加速度传感器和三轴磁阻传感器数据，得到描述于世界坐标系下的初始加速度矢量a_o＝[a_ox，a_oy，a_oz]^T和磁场矢量m_o＝[m_ox，m_oy，m_oz]^T；

2)在k时刻采集三轴陀螺仪(2-1)、三轴加速度传感器(2-2)和三轴磁阻传感器(2-3)数据，得到描述于装置坐标系下的当前旋转角速度矢量ω_t(k)＝[ω_tx(k)，ω_ty(k)，ω_tz(k)]^T、加速度矢量a_t(k)＝[a_tx(k)，a_ty(k)，a_tz(k)]^T和磁场矢量m_t(k)＝[m_tx(k)，m_ty(k)，m_tz(k)]^T；

3)构建系统状态方程(3)：

定义系统状态向量为：

X(k)＝[q^T(k)，μ^T(k)，a_b^T(k)]^T    (1)

其中：q(k)＝[q₀(k)，q₁(k)，q₂(k)，q₃(k)]^T为描述当前姿态同初始姿态之间，即装置坐标系同世界参考坐标系之间相对关系的旋转单位四元数矢量，μ(k)＝[μ_x(k)，μ_y(k)，μ_z(k)]^T为三轴陀螺仪的累积误差矢量，a_b(k)＝[a_bx(k)，a_by(k)，a_bz(k)]^T为描述于世界坐标系中的装置自身的运动加速度矢量。

用以描述装置坐标系同世界坐标系之间关系的单位四元数q(k)满足以下离散差分方程：

$q\left(k\right)=[I_{4\times 4}+\frac{\mathrm{\Delta t}}{2}M\left(\omega \left(k\right)\right)]q(k-1)---\left(2\right)$

其中，ω(k)＝[ω_x(k)，ω_y(k)，ω_z(k)]^T为装置在k时刻的瞬时旋转角速度矢量，

描述于装置坐标系；矩阵I为相应阶次的单位矩阵；Δt为采样周期。旋转角速度矢量ω(k)可由三轴陀螺仪感知得到，然而必然引入陀螺仪噪声分量，即：

ω_t(k)＝ω(k)+n_ω1+n_ω2+μ(k)                (3)

由上式可以看出，陀螺仪噪声矢量由三部分组成：电磁噪声n_ω1为各轴相互独立、且服从标准方差为σ_ω1的零均值高斯白噪声；漂移扭矩噪声n_ω2为各轴相互独立、且服从标准方差为的零均值高斯白噪声；μ(k)为截至到k时刻，各轴陀螺仪的累积误差矢量。将(2)式代入(1)式，并经过整理可得：

$q\left(k\right)=[I_{4\times 4}+\frac{\mathrm{\Delta t}}{2}M({\omega }_t\left(k\right)-n_{\omega 1}-n_{\omega 2}-\mu \left(k\right))]q(k-1)$

$=[I_{4\times 4}+\frac{\mathrm{\Delta t}}{2}M\left({\omega }_t\left(k\right)\right]q(k-1)-\frac{\mathrm{\Delta t}}{2}M\left(\mu \left(k\right)\right)q(k-1)-\frac{\mathrm{\Delta t}}{2}M(n_{\omega 1}+n_{\omega 2})q(k-1)---\left(4\right)$

$=[I_{4\times 4}+\frac{\mathrm{\Delta t}}{2}M\left({\omega }_t\left(k\right)\right]q(k-1)-\frac{\mathrm{\Delta t}}{2}E\left(q(k-1)\right)\mu \left(k\right)-\frac{\mathrm{\Delta t}}{2}E\left(q(k-1)\right)(n_{\omega 1}+n_{\omega 2})$

其中，$E\left(q(k-1)\right)=\left(\begin{array}{ccc}-q_1(k-1)& -q_2(k-1)& -q_3(k-1)\\ q_0(k-1)& -q_3(k-1)& q_2(k-1)\\ q_3(k-1)& q_0(k-1)& -q_1(k-1)\\ -q_2(k-1)& q_1(k-1)& q_0(k-1)\end{array}\right).$

累积误差μ(k)可建模为由标准方差为的零均值高斯白噪声n_ω3驱动的随机游动噪声，即：

μ(k)＝μ(k-1)+n_ω3                    (5)

装置本体自身加速度矢量a_b(k)描述于世界参考坐标系，其变化可以视为具有适当方差的零均值高斯白噪声其合理性一方面在于采样周期较小，相邻两时刻间的加速度变化可以理解为随机扰动，而方差则描述了这种扰动的幅度；另一方面，在没有其它信息可用的情况下，可以认为k时刻的加速度以较高概率保持k-1时刻的值，并在一定范围内服从高斯分布。因此：

$a_b\left(k\right)=a_b(k-1)+n_{a_b}---\left(6\right)$

综合(4)、(5)和(6)式，可得系统状态方程为：

$X\left(k\right)=F(X(k-1),{\omega }_t\left(k\right))+n_x\left(q(k-1)\right)$

$=\left(\begin{array}{ccc}{I}_{4\times 4}+\frac{\mathrm{\Delta t}}{2}M\left({\omega }_t\left(k\right)\right)& -\frac{\mathrm{\Delta t}}{2}E\left(q(k-1)\right)& 0_{4\times 3}\\ 0_{3\times 4}& I_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 4}& 0_{3\times 3}& I_{3\times 3}\end{array}\right)X(k-1)+\left(\begin{array}{ccc}-\frac{\mathrm{\Delta t}}{2}E\left(q(k-1)\right)& 0_{4\times 3}& 0_{4\times 3}\\ 0_{3\times 4}& \mathrm{\Delta t}& 0_{3\times 3}\\ 0_{3\times 4}& 0_{3\times 3}& I_{3\times 3}\end{array}\right)n_{\mathrm{Gx}}---\left(7\right)$

其中为系统噪声向量，其协方差矩阵Q计算为：

$Q=\left(\begin{array}{ccc}({\sigma }_{\omega 1}^2+{\sigma }_{\omega 2}^2\mathrm{\Delta t}{I}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& {\sigma }_{\omega 3}^2\mathrm{\Delta t}{I}_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& {\sigma }_{a_b}^2{I}_{3\times 3}\end{array}\right)---\left(8\right)$

4)依据加速度矢量a_t(k)和磁场矢量m_t(k)构建系统观测方程(4)：

k时刻的加速度矢量a_t(k)为重力加速度矢量和装置自身加速度矢量的复合。在以往方法中，往往假设装置自身加速度矢量远远小于重力加速度矢量，并因此忽略装置的自身加速度矢量，以期便于利用相关线性理论和方法对其进行处理。尽管在大多数场合下，这种假设是成立的，然而在装置进行大幅度变化运动时，往往会导致较大估计误差的产生，从而影响系统性能。本发明对装置自身加速度矢量和重力加速度矢量进行综合考虑，这一点不但体现在系统状态向量的定义和系统状态方程的构建中，最为重要的是体现在观测方程的构建上。

理想情况下，描述于装置坐标系的加速度矢量a_t(k)同描述于世界坐标系的初始加速度矢量(可以理解为纯粹的重力加速度矢量)a_o、装置自身的运动加速度矢量a_b(k)之间存在以下关系：

$a_t^q\left(k\right)=q^{-1}\left(k\right)\otimes [a_b^q\left(k\right)+a_o^q]\otimes q\left(k\right)=q^{-1}\left(k\right)\otimes a_{\mathrm{ob}}^1\left(k\right)\otimes q\left(k\right)---\left(9\right)$

其中，a_ob(k)＝a_o+a_b(k)；各矢量的上标q表示由0和该矢量所构成的矢量四元数，表示四元数乘积。实际上，由于存在感知误差，上式通常难以得到满足，四元数q(k)也只是在矢量a_t(k)和a_ob(k)具有相同幅值的情况下，才能对二者所在坐标系之间的相对关系进行正确描述。由于待估量a_b(k)的存在，q(k)不能同时保证二者具有相同的幅值和方向，为此本发明针对加速度矢量的方向和幅度分别建立观测方程。首先，无论a_ob(k)幅值如何，其必然和矢量a_t(k)具有相同方向，即二者满足：

${\hat{a}}_t^q\left(k\right)=q^{-1}\left(k\right)\otimes {\hat{a}}_{\mathrm{ob}}^q\left(k\right)\otimes q\left(k\right)+n_{\hat{a}}---\left(10\right)$

其中为相应矢量的归一化四元数，即：

${\hat{a}}_t^q\left(k\right)=\frac{1}{\left|\right|a_t\left(k\right)\left|\right|}[0,a_t^T\left(k\right)]=[0,{\hat{a}}_t^T\left(k\right)]=[0,{\hat{a}}_{\mathrm{tx}}\left(k\right),{\hat{a}}_{\mathrm{ty}}\left(k\right),{\hat{a}}_{\mathrm{tz}}\left(k\right)]---\left(11\right)$

${\hat{a}}_{\mathrm{ob}}^q\left(k\right)=\frac{1}{\left|\right|a_{\mathrm{ob}}\left(k\right)\left|\right|}[0,a_{\mathrm{ob}}^T\left(k\right)]=[0,{\hat{a}}_{\mathrm{ob}}^T\left(k\right)]=[0,{\hat{a}}_{\mathrm{obx}}\left(k\right),{\hat{a}}_{\mathrm{oby}}\left(k\right),{\hat{a}}_{\mathrm{obz}}\left(k\right)]---\left(12\right)$

为方差为的零均值高斯白噪声。(10)式两端同时左乘q(k)/2，并对其进行整理可得：

$Z_{\hat{a}}\left(k\right)=\frac{1}{2}q\left(k\right)\otimes {\hat{a}}_t^q\left(k\right)-\frac{1}{2}{\hat{a}}_{\mathrm{ob}}^q\left(k\right)\otimes q\left(k\right)+\frac{1}{2}q\left(k\right)\otimes n_{\hat{a}}$

$=\left(\begin{array}{cc}0& -\frac{1}{2}{[{\hat{a}}_t\left(k\right)+{\hat{a}}_{\mathrm{ob}}\left(k\right)]}^T\\ \frac{1}{2}[{\hat{a}}_t\left(k\right)+{\hat{a}}_{\mathrm{ob}}\left(k\right)]& \frac{1}{2}{[{\hat{a}}_t\left(k\right)-{\hat{a}}_{\mathrm{ob}}\left(k\right)]}_{\times }\end{array}\right)q\left(k\right)+\frac{1}{2}E\left(q\left(k\right)\right)n_{\hat{a}}$

$=A({\hat{a}}_t\left(k\right),{\hat{a}}_{\mathrm{ob}}\left(k\right))q\left(k\right)+\frac{1}{2}E\left(q\left(k\right)\right)n_{\hat{a}}---\left(13\right)$

其中，[□]_×表示由相应向量定义的反对称矩阵。

矢量a_t(k)和a_ob(k)之间的幅值差异，可描述为具有较小方差σ_|a|的零均值白噪声n_|a|，由此可建立方程：

$Z_{\left|a\right|}\left(k\right)=\left|\right|a_t\left(k\right)\left|\right|-\left|\right|a_{\mathrm{ob}}\left(k\right)\left|\right|+n_{\left|a\right|}$

$=\sqrt{a_{\mathrm{tx}}^2\left(k\right)+a_{\mathrm{ty}}^2\left(k\right)+a_{\mathrm{tz}}^2\left(k\right)}-\sqrt{a_{\mathrm{obx}}^2\left(k\right)+a_{\mathrm{oby}}^2\left(k\right)+a_{\mathrm{obz}}^2\left(k\right)}+n_{\left|a\right|}---\left(14\right)$

磁场矢量m_t(k)和m_o为同一地磁矢量在不同坐标系下的描述，同加速度矢量相似，二者也应该具有一致的方向和相同的幅值。由于m_t(k)为传感器检测矢量，此处仅根据二者单位方向矢量之间的关系构建观测方程如式(15)，其推导过程同加速度矢量类似。

$Z_{\hat{m}}\left(k\right)=\left(\begin{array}{cc}0& -\frac{1}{2}{[{\hat{m}}_t\left(k\right)+{\hat{m}}_o]}^T\\ \frac{1}{2}{[{\hat{m}}_t\left(k\right)+{\hat{m}}_o]}^T& \frac{1}{2}{\left[{[{\hat{m}}_t\left(k\right)-{\hat{m}}_o]}^T\right]}_{\times }\end{array}\right)q\left(k\right)+\frac{1}{2}E\left(q\left(k\right)\right)n_{\hat{m}}$

$=A\left({\hat{m}}_t\left(k\right)\right)q\left(k\right)+\frac{1}{2}E\left(q\left(k\right)\right)n_{\hat{m}}---\left(15\right)$

其中，分别为m_t(k)和m_o的归一化单位方向矢量；为方差为的零均值白噪声。

综合式(13)、(14)、(15)，可得系统的总体观测方程为：

$Z\left(k\right)=\left(\begin{array}{c}{Z}_{\hat{a}}\left(k\right)\\ Z_{\left|a\right|}\left(k\right)\\ Z_{\hat{m}}\left(k\right)\end{array}\right)=\left(\begin{array}{c}\frac{1}{2}\left[A({\hat{a}}_t\left(k\right),{\hat{a}}_{\mathrm{ob}}\left(k\right))\right]q\left(k\right)\\ \left|\right|a_t\left(k\right)\left|\right|-\left|\right|a_{\mathrm{ob}}\left(k\right)\left|\right|\\ \frac{1}{2}\left[A\left({\hat{m}}_t\left(k\right)\right)\right]q\left(k\right)\end{array}\right)+\left(\begin{array}{ccc}\frac{1}{2}E\left(q\left(k\right)\right)& 0_{4\times 1}& 0_{4\times 3}\\ 0_{1\times 3}& 1& 0_{1\times 3}\\ 0_{4\times 3}& 0_{4\times 1}& \frac{1}{2}E\left(q\left(k\right)\right)\end{array}\right)n_{\mathrm{Gz}}$

$=G\left(X\left(k\right)\right)+D\left(X\left(k\right)\right)n_{\mathrm{Gz}}---\left(16\right)$

式中为观测噪声矢量，其协方差矩阵为$R=\left(\begin{array}{ccc}{\sigma }_{\hat{a}}^2{I}_{3\times 3}& 0_{3\times 1}& 0_{3\times 3}\\ 0_{1\times 3}& {\sigma }_{\left|a\right|}^2& 0_{1\times 3}\\ 0_{3\times 3}& 0_{3\times 1}& {\sigma }_{\hat{m}}^2{I}_{3\times 3}\end{array}\right).$

由系统状态方程和观测方程可以看出：系统具有非线性性质，无论是系统噪声还是观测噪声也均同当前时刻的状态向量相关，因此不能采用传统的线性Kalman滤波理论实现相关状态的估计；同时，观测方程的泰勒展开式由于不能保证收敛而无法使用扩展Kalman滤波方法。因此，本发明采用UKF技术实现系统状态的估计过程，并对系统噪声和观测噪声的协方差矩阵依据相关状态向量值进行实时更新。为此，本发明仍包括以下几个步骤：

5)系统状态Sigma点采样(5)。根据k-1时刻的系统状态X(k-1/k-1)和协方差矩阵P(k-1/k-1)进行Sigma点采样(依据统计量进行样本点抽取的一种技术，具体见文献Julier，Simon J.and Jeffery K.Uhlmann.ANew Extension of the Kalman Filter to Nonl inear Systems.TheProceedings of AeroSense：The 1lth International Symposium onAerospace/Defense Sensing，Simulation and Controls，Multi SensorFusion，Tracking and Resource Management 11，SPIE，1997)，得到21个点样本为：

X_s0＝X(k-1/k-1)

$X_{\mathrm{si}}=X(k-1/k-1)+{\left(\sqrt{3P(k-1/k-1)}\right)}_i$i＝1，…，10(17)

$X_{\mathrm{si}}=X(k-1/k-1)+{\left(\sqrt{3P(k-1/k-1)}\right)}_{i-10}$i＝11，…，20

其中为矩阵平方根矩阵的第i列。

6)UKF预测(6)。根据系统状态方程(7)，分别依据21个Sigma点样本进行状态预测：

X_spi＝F(X_si，ω_t(k))i＝0，…，21    (18)

利用上述采样预测值确定系统状态向量和协方差矩阵的最终预测值为：

$X(k/k-1)=\underset{i=0}{\overset{21}{\Sigma }}{w}_i{X}_{\mathrm{spi}}---\left(19\right)$

$P(k/k-1)=\underset{i=0}{\overset{21}{\Sigma }}{w}_i[X_{\mathrm{spi}}-X(k/k-1)]{[X_{\mathrm{spi}}-X(k/k-1)]}^T+Q_{k-1}---\left(20\right)$

$Q_{k-1}=\left(\begin{array}{ccc}\frac{1}{4}({\sigma }_{\omega 1}^2+{\sigma }_{\omega 2}^2\mathrm{\Delta t})[\mathrm{tr}\left(M_{k-1}\right)I_{4\times 4}-M_{k-1}]& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& {\sigma }_{\omega 3}^2\mathrm{\Delta t}{I}_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& {\sigma }_{a_b}^2{I}_{3\times 3}\end{array}\right)---\left(21\right)$

M_k-1＝q(k-1/k-1)q^T(k-1/k-1)+P_q(k-1/k-1)        (22)

其中，w_i为相关点采样的权值：对于X_s0而言，对于其它点样本，P_q(k-1/k-1)为矩阵P(k-1/k-1)中相应于四元数向量的协方差子阵；矩阵Q_k-1的计算推导过程请参阅文献Choukroun D.，Bar-Itzhack I.，Oshman Y.，Novel quaternion Kalman filter，IEEE Transactions onAerospace and Electronic Systems，2006，Vol.42，No.1：174-190。

7)UKF更新(7)。对于Sigma点预测X_spi，令q_i(k/k-1)为由向量X_spi前四个元素得到的归一化四元数，根据观测方程其观测值计算为：

Z_i(k)＝G(X_spi(k/k-1))i＝0，…，21    (23)

而最终观测值计算为：

$Z\left(k\right)=\underset{i=0}{\overset{21}{\Sigma }}{w}_i{Z}_i\left(k\right)---\left(24\right)$

系统状态向量和协方差矩阵的最终更新为：

X(k/k)＝X(k/k-1)-KZ(k)     (25)

P(k/k)＝P(k/k-1)-KP_ZZK^T    (26)

其中：

$K=P_{\mathrm{XZ}}{P}_{\mathrm{ZZ}}^{-1}---\left(27\right)$

$P_{\mathrm{ZZ}}=\underset{i=0}{\overset{21}{\Sigma }}{w}_i[Z_i\left(k\right)-Z\left(k\right)]{[Z_i\left(k\right)-Z\left(k\right)]}^T+\underset{i=0}{\overset{21}{\Sigma }}{w}_i{R}_i---\left(28\right)$

$P_{\mathrm{XZ}}=\underset{i=0}{\overset{21}{\Sigma }}{w}_i[X_{\mathrm{spi}}-X(k/k-1)]{[Z_i\left(k\right)-Z\left(k\right)]}^T---\left(29\right)$

$R_i=\left(\begin{array}{ccc}{D}_{\hat{a}i}& 0_{4\times 1}& 0_{4\times 4}\\ 0_{1\times 4}& {\sigma }_{\left|a\right|}^2& 0_{1\times 4}\\ 0_{4\times 4}& 0_{4\times 1}& D_{\hat{m}i}\end{array}\right)---\left(30\right)$

$D_{\hat{a}i}=\frac{1}{4}{\sigma }_{\hat{a}}^2[\mathrm{tr}\left(M_{\mathrm{ki}}\right)I_{4\times 4}-M_{\mathrm{ki}}-B\left({\hat{a}}_t\left(k\right)\right)M_{\mathrm{ki}}{B}^T\left({\hat{a}}_t\left(k\right)\right)]---\left(31\right)$

$D_{\hat{m}i}=\frac{1}{4}{\sigma }_{\hat{m}}^2[\mathrm{tr}\left(M_{\mathrm{ki}}\right)I_{4\times 4}-M_{\mathrm{ki}}-B\left({\hat{m}}_t\left(k\right)\right)M_{\mathrm{ki}}{B}^T\left({\hat{m}}_t\left(k\right)\right)]---\left(32\right)$

$M_{\mathrm{ki}}=q_i(k/k-1)q_i^T(k/k-1)+P_q(k/k-1)---\left(33\right)$

$B\left(r\right)=\left(\begin{array}{cc}0& -r^T\\ r& {\left[r\right]}_{\times }\end{array}\right)---\left(34\right)$

其中P_q(k/k-1)为矩阵P(k/k-1)中相应于四元数向量的协方差子阵，协方差矩阵R_i的计算推导过程请参阅文献Choukroun D.，Bar-Itzhack I.，Oshman Y.，Novel quaternion Kalman filter，IEEE Transactions onAerospace and Electronic Systems，2006，Vol.42，No.1：174-190。

8)欧拉角解算(8)：将X(k/k)中的四元数向量元素进行归一化处理，并根据旋转的四元数表示同欧拉角表示之间的关系将其转换为具有较为直观意义的俯仰角β、横滚角α和航向角γ：

$\left(\begin{array}{cc}\alpha =\mathrm{arctg}\left(\frac{2(q_2{q}_3-q_0{q}_1)}{q_0^2-q_1^2-q_2^2+q_3^2}\right)& \alpha \in (-\mathrm{180,180}]\\ \beta =\arcsin (-2(q_1{q}_3+q_0{q}_2))& \beta \in (-\mathrm{90,90}]\\ \gamma =\mathrm{arctg}\left(\frac{2(q_1{q}_2-q_0{q}_3)}{q_0^2+q_1^2-q_2^2-q_3^2}\right)& \gamma \in (-\mathrm{180,180}]\end{array}\right)---\left(35\right)$

本发明考虑加速度补偿和基于UKF的姿态跟踪方法适用于具有如图2所示架构的惯性姿态跟踪装置。该类装置以正交方式集成了三个加速度传感器、三个陀螺仪和三个磁场传感器，三组传感器所在的正交轴构成了装置坐标系。

依据上述具体步骤，对其中的具体实现细节进行如下说明：

1)在步骤(1)初始姿态标定过程中，由于各传感器均存在感知噪声，需要进行多次采样，对多次采样值进行平均后作为描述于世界坐标系下的初始加速度矢量a_o和磁场矢量m_o；

2)在步骤(3)系统状态方程构建过程中，陀螺仪电磁噪声n_ω1方差σ_ω1＝0.2；陀螺仪漂移扭矩噪声n_ω2方差中的参数σ_ω2＝4.0；激励陀螺仪累积误差的零均值高斯白噪声n_ω3的方差中的参数σ_ω3＝0.001；装置本身运动加速度的噪声方差设为重力加速度g。

3)在步骤(4)系统观测方程构建过程中，加速度矢量方差噪声n_|a|的方差σ_|a|＝0.05；磁场噪声矢量各分量的方差为

4)在UKF滤波过程中，系统状态向量X的初始值设置为：

X(0/0)＝[1，0，0，0，0，0，0，0，0，0]^T

协方差矩阵P的初始值设置为P(0/0)＝0.5I_10×10

5)本发明所述方法的系统采样和滤波周期为Δt＝30ms。

采用本发明考虑加速度补偿和基于UKF的姿态跟踪方法，可以取得以下效果：

一方面，在装置本体自身运动幅度较小的情况下，能够取得同传统Kalman滤波方法相似甚至更好的效果，这一点从图3、图4和图5所示的、装置分别沿X轴、Y轴和Z轴做纯旋转运动时的跟踪效果对比可以看出。图3为装置沿X轴做纯旋转运动时，本发明方法和传统Kalman滤波方法的姿态跟踪曲线图；其中，带有圆形标记的曲线为采用传统Kalman滤波技术得到的估计结果曲线；带有十字标记的曲线为采用本发明所得到的滤波估计结果曲线。图4为装置沿Y轴做纯旋转运动时，本发明方法和传统Kalman滤波方法的姿态跟踪曲线图；其中，带有圆形标记的曲线为采用传统Kalman滤波技术得到的估计结果曲线；带有十字标记的曲线为采用本发明所得到的滤波估计结果曲线。图5为装置沿Z轴做纯旋转运动时，本发明方法和传统Kalman滤波方法的姿态跟踪曲线图；其中，带有圆形标记的曲线为采用传统Kalman滤波技术得到的估计结果曲线；带有十字标记的曲线为采用本发明所得到的滤波估计结果曲线。

实际上，由于本发明方法对加速度进行了补偿，在一定程度上排除了导致传统方法误差产生的一个重要因素，因此，应该比传统方法具有更高的精度。只是在装置本体自身运动加速度较小的情况下，这种精度的改善幅度不大。

另一方面，当装置本体自身运动幅度较大时，本发明方法对于系统精度的提高效果是非常明显的。图6所示为当装置沿Y轴以较大加速度进行往返平移运动时，利用传统Kalman滤波技术和利用本发明方法所感知到的装置姿态的变化曲线，其中，带有星形标记的曲线为采用传统Kalman滤波技术得到的估计结果曲线；带有十字标记的曲线为采用本发明所得到的滤波估计结果曲线。理论上，当装置做纯平移运动时，其姿态保持不变，然而由于传统Kalman滤波技术忽略了装置本身自运动加速度对于感知过程的影响，将装置沿Y轴方向上的加速度归因于绕X轴或绕Z轴的旋转运动所致，从而导致如图6中所示的X轴方向和Z轴方向上的欧拉角具有较大偏差。本发明方法对装置本身自运动加速度进行了充分考虑和补偿，从而取得较为理想的跟踪效果。图7所示为装置进行上述运动时，利用本发明方法所提取出的装置本身的自运动加速度。