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

权利要求书

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)]×]]>

A(m^t(k))=0-12[m^t(k)+m^o]T12[m^t(k)+m^o]T12[[m^t(k)-m^o]T]×]]>

其中，[·]_×表示由相应向量定义的反对称矩阵；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))MkiBT(a^t(k))]---(17)]]>

Dm^i=14σm^2[tr(Mki)I4×4-Mki-B(m^t(k))MkiBT(m^t(k))]---(18)]]>

Mki=qi(k/k-1)qiT(k/k-1)+Pq(k/k-1)---(19)]]>

B(r)=0-rTr[r]×---(20)]]>

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

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