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

摘要

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

主权项

1.一种考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法，适用于以正交方式集成三轴陀螺仪、三轴加速度计和三轴磁场传感器的姿态感知装置；其特征在于，系统状态向量包含了装置本身的运动加速度，并对其进行滤波估计；包括以下步骤：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(k)=F(X(k-1),ωt(k))+nx(q(k-1))]]>=I4×4+Δt2M(ωt(k))-Δt2E(q(k-1))04×303×4I3×303×303×403×3I3×3X(k-1)---(2)]]>+-Δt2E(q(k-1))04×304×303×4Δt03×303×403×3I3×3nGx]]>式中Δt为采样周期，n_Gx为噪声矢量，其协方差矩阵为矩阵I为相应阶次的单位矩阵，σ_ω1、σ_ω2、σ_ω3和为相关噪声变量的标准方差常数；4)构建系统观测方程：Z(k)=Za^(k)Z|a|(k)Zm^(k)=12[A(a^t(k),a^ob(k))]q(k)||at(k)||-||aob(k)||12[A(m^t(k))]q(k)+12E(q(k))04×104×301×3101×304×304×112E(q(k))nGz]]>=G(X(k))+D(X(k))nGz---(3)]]>式中，a_ob(k)＝a_o+a_b(k)，对于任意三维矢量r＝[r_x，r_y，r_z]^T，为其归一化单位方向矢量，||r||为其幅值，矩阵：A(a^t(k),a^ob(k))=0-12[a^t(k)+a^ob(k)]T12[a^t(k)+a^ob(k)]12[a^t(k)-a^ob(k)]x]]>A(m^t(k))=0-12[m^t(k)+m^o]T12[m^t(k)+m^o]T12[[m^t(t)-m^o]T]x]]>其中，表示由相应向量定义的反对称矩阵；n_Gz为观测噪声矢量，其协方差矩阵为σ_|a|和为相关噪声变量的标准方差常数；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)=Σi=021wiXspi---(5)]]>P(k/k-1)=Σi=021wi[Xspi-X(k/k-1)][Xspi-X(k/k-1)]T+Qk-1---(6)]]>Qk-1=14(σω12+σω22Δt)[tr(Mk-1)I4×4-Mk-1]03×303×303×3σω32ΔtI3×303×303×303×3σab2I3×3---(7)]]>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(k)=Σi=021wiZi(k)---(10)]]>系统状态向量和协方差矩阵的最终更新为：X(k/k)＝X(k/k-1)-KZ(k)     (11)P(k/k)＝P(k/k-1)-KP_ZZK^T    (12)其中：K=PXZPZZ-1---(13)]]>PZZ=Σi=021wi[Zi(k)-Z(k)][Zi(k)-Z(k)]T+Σi=021wiRi---(14)]]>PXZ=Σi=021wi[Xspi-X(k/k-1)][Zi(k)-Z(k)]T---(15)]]>Ri=Da^i04×104×401×4σ|a|201×404×404×1Dm^i---(16)]]>Da^i=14σa^2[tr(Mki)I4×4-Mki-B(a^t(k))Mki-BT(a^t(k))]---(17)]]>Dm^i=14σm^2[tr(Mki)I4×4-Mki-B(m^t(k))Mki-BT(m^t(k))]---(18)]]>Mki=qi(k/k-1)qiT(k/k-1)+Pq(k/k-1)---(19)]]>B(r)=0-rTr[r]x---(20)]]>其中P_q(k/k-1)为矩阵P(k/k-1)中相应于四元数向量的协方差子阵；8)将X(k/k)中的四元数向量元素进行归一化处理，并利用四元数表示同欧拉角表示之间的关系将其转换为具有较为直观意义的俯仰角、横滚角和航向角。