用于惯性-地磁组合的快速扩展卡尔曼算法的制作方法

本发明涉及一种用于惯性-地磁组合的快速扩展卡尔曼算法。

背景技术：

惯性-地磁组合是一种传感器组合，用于实现载体的姿态解算，相比于视觉、超声、微波等其他定姿技术，惯性-地磁组合的突出优点是其姿态解算不需依赖任何外部人工源，因此其使用不受区域限制，并且由于其广泛采用小体积的mems(微机械系统)传感器，这就使其非常便于携带，以上两种原因使得惯性-地磁组合特别适合应用于户外人体运动跟踪等领域。惯性-地磁组合的硬件主要由三轴加速度计、三轴陀螺仪和三轴地磁传感器等三种传感器构成，软件程序主要为姿态解算程序，用于实现传感器信息融合进而获得载体运动时的姿态变化。所谓的“姿态”指的是载体坐标系相对于参考坐标系之间的坐标转换关系，通常用欧拉角、四元数、旋转矩阵或等效旋转矢量等表示，但由于四元数表示形式比较紧凑，而且非奇异(即不存在万向节锁问题)，因此应用最为广泛。参考坐标系通常选为当地地理坐标系，也可选为惯性坐标系或地球坐标系。目前常用的姿态解算算法主要有两类，即扩展卡尔曼算法和互补滤波算法。此外，其他一些基于统计线性化技术的算法，比如粒子滤波算法、无迹滤波算法等，或者基于最小二乘技术的算法，比如最优卡尔曼算法、扩展quest算法等，也都能被用作姿态解算算法，只不过鉴于惯性-地磁组合的高采样率和低运算能力的特点，这些算法并不往往适用于惯性-地磁组合。

已经有研究表明，扩展卡尔曼算法的姿态解算精度比互补滤波算法要高，然而计算速度却比互补滤波算法要慢。考虑到精度问题，扩展卡尔曼算法目前已经成为应用最广的算法，然而计算量较大的瓶颈进一步限制了其更广泛的应用。

技术实现要素：

本发明所要解决的技术问题是：在不损失精度的前提下，为了进一步提高扩展卡尔曼算法的姿态解算速度，本发明提供一种用于惯性-地磁组合的快速扩展卡尔曼算法，该算法利用加速度计和地磁传感器的输出信息构建两个正交矢量，随后利用这两个正交矢量简化了卡尔曼增益矩阵，使得其表达式中不含有(求解非常耗时的)逆矩阵，从而大幅度的提高了扩展卡尔曼算法的计算速度，但是仍然基本保持了原有的姿态解算精度。

本发明解决其技术问题所要采用的技术方案是：一种用于惯性-地磁组合的快速扩展卡尔曼算法，基本思想是，通过构建两个相互正交的观测矢量，去除卡尔曼增益矩阵中的逆矩阵，从而提高扩展卡尔曼算法的计算速度。具体实施步骤如下：

步骤1：构建重力加速度矢量gr和地磁场矢量mr在当地地理坐标系下的表示，即：

gr＝[0,0,-1]^t，mr＝[cos(90°-θgm),0,-sin(90°-θgm)]^t(1)

其中，θgm为gr和mr空间夹角，然后利用gr和mr构建两个空间正交矢量，即：

r1′＝(gr+mr)/[2cos(θgm/2)]，r′2＝(gr-mr)/[2sin(θgm/2)](2)

为了使得新构建的正交矢量的测量轴与陀螺仪的测量轴一致，再将r1′和r′2进行空间旋转，即：

其中，qrot＝cos(θgm/4)+sin(θgm/4)·(b′1×b′2)/|b′1×b′2|。后续步骤将利用r1和r2作为姿态解算时的参考矢量。

步骤2：在每个采样时刻k，由三轴加速度计和三轴地磁传感器的输出分别构建r1和r2在载体坐标系下的观测值，即gb和mb。和步骤1类似，仍然利用gb和mb构建两个正交矢量，即：

b′1＝(gb+mb)/[2cos(θgm/2)]，b′2＝(gb-mb)/[2sin(θgm/2)](4)

并随后对其进行空间旋转以使得旋转后的两个正交矢量的测量轴与陀螺仪一致，即：

扩展卡尔曼算法的过程方程和观测方程通常分别具有如下形式，

过程方程为：

观测方程为：

其中，fs为采样频率；为由k时刻的陀螺仪三轴向输出ωx、ωy和ωz构成的三维反对称阵；φk命名为状态转换矩阵；qk和qk+1分别为k时刻和k+1时刻由四元数表示的载体姿态；wk为高斯白噪声，其协方差其中i4×4为四维单位矩阵，σw为陀螺仪每个轴向上的输出噪声的标准差，进一步地，如果定义姿态的一步预测估计为qk+1/k＝φkqk＝[q0q1iq2jq3k]，那么：

旋转矩阵

和为互不相关的观测噪声，其协方差分别为，和其中σa和σm分别为b1和b2在每个轴向上的输出噪声的标准差。

扩展卡尔曼算法的姿态递推迭代方程为

式中，卡尔曼增益矩阵其中pk+1/k为(利用k时刻的姿态及k+1时刻的陀螺仪输出对k+1时刻的姿态进行一步预测的)预测协方差，为线性化后的观测矩阵，为观测方程的观测误差协方差。

卡尔曼增益矩阵因含有逆矩阵，因此计算量较大，这也是扩展卡尔曼算法的计算量较大的主要原因。为此，在保证卡尔曼增益矩阵的计算精度的前提下，步骤3～7将充分利用由步骤1和2得到的正交矢量b1、b2、r1和r2，实现卡尔曼增益矩阵的简化，进而减少扩展卡尔曼算法的计算量。

步骤3：利用r1和r2,将fk+1化简为

步骤4：令姿态验后估计误差协方差pk/k＝α²i4×4，本说明书的最后部分将具体解释上述误差协方差矩阵定义的合理性。利用pk/k，将pk+1/k简化为

步骤5：令其中max()函数表示取括号内的两者的最大值，然后将矩阵简化为

其中,

由于δk和[max(q1,q2,q3,q0)]⁴是同阶次，而且c/4是非常小的一个量，因而c/4δk可以合理地略去。在略去c/4δk之后，矩阵的逆矩阵简化为

其中，

步骤6：将矩阵简化为

步骤7：利用步骤5和6的结果将卡尔曼增益简化为

并最终将扩展卡尔曼算法的姿态递推迭代方程简化为

其中，

返回步骤2，直至用户终止运算。

关于pk/k＝α²i4×4合理性的解释：由于初始时刻时载体是静止的，因此可以利用常见的单采样点姿态解算算法(single-framedeterminationalgorithm)，比如quest算法(quaternionestimatoralgorithm)，解算出载体的初始姿态。此时扩展卡尔曼算法的初始姿态估计误差可以合理地设置为：

如果忽略高阶项，可以得到：

其中，

利用上述结果，pk/k可以由p0/0迭代计算出

本发明的有益效果是：本发明提供的一种用于惯性-地磁组合的快速扩展卡尔曼算法，在保证扩展卡尔曼算法的姿态解算精度的同时，大幅度的提高其计算速度，从而产生“姿态解算精度比互补滤波算法高，计算速度与互补滤波算法相当”的有益效果，这将有利于扩展卡尔曼算法全面替代互补滤波算法，进而完成姿态解算算法的数学性统一，也为惯性-地磁组合的更广泛应用打下良好的基础。

附图说明

下面结合附图和实施例对本发明作进一步说明。

图1是本发明快速卡尔曼算法的基本流程图。

图2是本发明实验环境布置示意图。

图3是本发明快速扩展卡尔曼算法的姿态解算误差。

图4是现有技术所采用的扩展卡尔曼算法的姿态解算误差。

具体实施方式

现在结合附图对本发明作详细的说明。此图为简化的示意图，仅以示意方式说明本发明的基本结构，因此其仅显示与本发明有关的构成。

如图1所示，本发明的一种用于惯性-地磁组合的快速扩展卡尔曼算法，具体实施步骤如下：

步骤1：构建重力加速度矢量gr和地磁场矢量mr在当地地理坐标系下的表示，即：

gr＝[0,0,-1]^t，mr＝[cos(90°-θgm),0,-sin(90°-θgm)]^t(1)

其中，θgm为gr和mr空间夹角，然后利用gr和mr构建两个空间正交矢量，即：

r1′＝(gr+mr)/[2cos(θgm/2)]，r′2＝(gr-mr)/[2sin(θgm/2)](2)

为了使得新构建的正交矢量的测量轴与陀螺仪的测量轴一致，再将r1′和r′2进行空间旋转，即：

其中，qrot＝cos(θgm/4)+sin(θgm/4)·(b′1×b′2)/|b′1×b′2|。后续步骤将利用r1和r2作为姿态解算时的参考矢量。

步骤2：在每个采样时刻k，由三轴加速度计和三轴地磁传感器的输出分别构建r1和r2在载体坐标系下的观测值，即gb和mb。和步骤1类似，仍然利用gb和mb构建两个正交矢量，即：

b′1＝(gb+mb)/[2cos(θgm/2)]，b′2＝(gb-mb)/[2sin(θgm/2)](4)

并随后对其进行空间旋转以使得旋转后的两个正交矢量的测量轴与陀螺仪一致，即：

扩展卡尔曼算法的过程方程和观测方程通常分别具有如下形式，

过程方程为：

观测方程为：

其中，fs为采样频率；为由k时刻的陀螺仪三轴向输出ωx、ωy和ωz构成的三维反对称阵；φk命名为状态转换矩阵；qk和qk+1分别为k时刻和k+1时刻由四元数表示的载体姿态；wk为高斯白噪声，其协方差其中i4×4为四维单位矩阵，σw为陀螺仪每个轴向上的输出噪声的标准差，进一步地，如果定义姿态的一步预测估计为qk+1/k＝φkqk＝[q0q1iq2jq3k]，那么：

旋转矩阵：

和为互不相关的观测噪声，其协方差分别为，和其中σa和σm分别为b1和b2在每个轴向上的输出噪声的标准差。

扩展卡尔曼算法的姿态递推迭代方程为

式中，卡尔曼增益矩阵其中pk+1/k为(利用k时刻的姿态及k+1时刻的陀螺仪输出对k+1时刻的姿态进行一步预测的)预测协方差，为线性化后的观测矩阵，为观测方程的观测误差协方差。

卡尔曼增益矩阵因含有逆矩阵，因此计算量较大，这也是扩展卡尔曼算法的计算量较大的主要原因。为此，在保证卡尔曼增益矩阵的计算精度的前提下，步骤3～7将充分利用由步骤1和2得到的正交矢量b1、b2、r1和r2，实现卡尔曼增益矩阵的简化，进而减少扩展卡尔曼算法的计算量。

步骤3：利用r1和r2,将fk+1化简为

步骤4：令姿态验后估计误差协方差pk/k＝α²i4×4，本说明书的最后部分将具体解释上述误差协方差矩阵定义的合理性。利用pk/k，将pk+1/k简化为

步骤5：令其中max()函数表示取括号内的两者的最大值，然后将矩阵简化为

其中,

由于δk和[max(q1,q2,q3,q0)]⁴是同阶次，而且c/4是非常小的一个量，因而c/4δk可以合理地略去。在略去c/4δk之后，矩阵的逆矩阵简化为

其中，

步骤6：将矩阵简化为

步骤7：利用步骤5和6的结果将卡尔曼增益简化为

并最终将扩展卡尔曼算法的姿态递推迭代方程简化为

其中，

返回步骤2，直至用户终止运算。

关于pk/k＝α²i4×4合理性的解释：由于初始时刻时载体是静止的，因此可以利用常见的单采样点姿态解算算法(single-framedeterminationalgorithm)，比如quest算法(quaternionestimatoralgorithm)，解算出载体的初始姿态。此时扩展卡尔曼算法的初始姿态估计误差可以合理地设置为：

如果忽略高阶项，可以得到：

其中，

利用上述结果，pk/k可以由p0/0迭代计算出

为了验证本发明提出的快速扩展卡尔曼算法的有效性，进行了如下实验：如图2所示，将惯性-地磁组合通过扎带固定于木棍的一端，将作为姿态参考(其输出被认为是载体的真实姿态，从而被用于评估扩展卡尔曼算法的姿态估计误差)的电磁跟踪系统的传感器通过扎带固定于木棍的另一端，电磁跟踪系统的激磁源置于木棍的附近。惯性-地磁组合及电磁跟踪系统周边无铁磁干扰。每个轴向上的陀螺仪、加速度计以及地磁传感器的输出噪声的标准差分别为σw＝0.006rad/s，σa＝0.008m/s²，σm＝0.001gauss。惯性-地磁组合及电磁跟踪系统的采样率分别为256hz及240hz。在惯性-地磁组合开始运动前，先将其放置一小段时间，以使得陀螺仪等传感器的输出趋于稳定。随后通过人手的作用力使木棍在三维空间内运动一段时间，最后再将其放置回原处。本专利提出的快速扩展卡尔曼算法以及目前采用的扩展卡尔曼算法均编写成matlab程序并运行于同一台电脑。图3和图4所示分别为两种算法针对上述运动过程的姿态解算误差变化时间曲线。表1为两种算法的计算耗时(每种算法重复运行10次，表中所示为10次的平均值)。

表1两种姿态解算算法的耗时对比

由图3和图4以及表1可以明显得出结论：本专利提出的快速扩展卡尔曼算法的姿态解算精度与目前采用的扩展卡尔曼算法相当，但是计算耗时明显比后者少。

以上述依据本发明的理想实施例为启示，通过上述的说明内容，相关的工作人员完全可以在不偏离本发明的范围内，进行多样的变更以及修改。本项发明的技术范围并不局限于说明书上的内容，必须要根据权利要求范围来确定其技术性范围。