[发明专利]一种用于提高惯性-地磁组合静动态综合性能的方法

说明书

本发明提供一种基于卡尔曼滤波器和隐马尔科夫模型的用于提高惯性‑地磁组合综合性能的方法，引入一种新的基于卡尔曼和隐马尔科夫模型的加权和动态调整机制，从而使得在动态条件下扩展卡尔曼算法和互补滤波算法能够完全依靠三轴陀螺仪实现姿态解算，而在静态条件下又能够主要依靠加速度计和地磁传感器实现姿态解算，保证了较好的动态性能和静态性能，从而提高惯性‑地磁组合的综合性能。

技术领域

本发明涉及一种基于卡尔曼滤波器和隐马尔科夫模型的用于提高惯性-地磁组合综合性能的方法。

背景技术

惯性-地磁组合是一种传感器组合，主要由三轴加速度计、三轴陀螺仪和三轴地磁传感器构成，用于实现载体的姿态测量。所谓的“姿态”指的是载体坐标系相对于参考坐标系之间的坐标转换关系，通常用欧拉角、四元数或旋转矩阵等表示。参考坐标系通常选为当地地理坐标系。惯性-地磁组合需要依靠其姿态解算算法来融合其传感器数据进而计算出载体的姿态，目前常用的姿态解算算法主要有两类，即扩展卡尔曼算法和互补滤波算法。一般来说，只要给定初始姿态，然后利用方向余弦算法等算法通过融合三轴陀螺仪输出数据也能够解算出姿态(命名为姿态1)，并且解算出的姿态不受载体线加速度影响，并因此具有很好的动态精度，然而由于方向余弦算法等算法通过角速度的积分来获取姿态的本质，陀螺仪的输出误差不可避免的会被积分并且叠加进姿态解算结果中，这就使得所解算出的姿态会随时间发散并因此具有很差的长时间静态精度。另一方面，利用矢量定姿算法，比如QUEST算法等，通过融合加速度计和地磁传感器输出数据也能够解算出姿态(命名为姿态2)，并且所解算出的姿态不受误差积分的影响并因此具有很好的静态精度，然而由于在动态条件下，加速度计所测得的加速度既包含重力加速度也包含载体线加速度，即加速度计的输出是上述两种加速度的叠加，这就使得所解算出的姿态受载体线加速度影响并因此具有很差的动态精度。扩展卡尔曼算法和互补滤波算法所解算出的姿态是姿态1和姿态2的加权和，其目的是对动态性能和静态性能做折中处理，从而使惯性-地磁组合能够兼具较好的动态和静态性能，但是性能折中会造成动态性能和静态性能均下降。

发明内容

本发明所要解决的技术问题是：为了解决性能折中造成的动态性能和静态性能均下降的问题，本发明提供一种基于卡尔曼滤波器和隐马尔科夫模型的用于提高惯性-地磁组合综合性能的方法，引入一种新的基于卡尔曼和隐马尔科夫模型的加权和动态调整机制，从而使得在动态条件下扩展卡尔曼算法和互补滤波算法能够完全依靠三轴陀螺仪实现姿态解算，而在静态条件下又能够主要依靠加速度计和地磁传感器实现姿态解算。

目前最具竞争力的动态调整加权和的方法有两种，这两种方法的基本思想是类似的。

第一种方法是设置陀螺仪的权值为

其中a_i为由三轴加速度输出构成的三维矢量，g为当地重力加速度矢量。α为某一阈值。事实上，|||a_i||-||g|||表示的是载体线加速度矢量模的估计。

另一种方法为

第一种方法为离散式调整方法，即权值要么为“1”，要么为“0”，而第二种方法为连续式调整方法。对于第一种方法，在加速度计输出噪声较大时，由于其苛刻的判断载体是否静止的条件，使得在静态条件下其更倾向于依靠陀螺仪实现姿态解算，因而在采样速率偏低时容易使扩展卡尔曼算法或互补滤波算法发散。对于第二种方法，即使负指数函数的输出能够随着输入的增大而快速衰减，但是在载体线加速度存在但偏小时，该函数仍不能使输出衰减至0，从而使扩展卡尔曼算法或互补滤波算法在此情形下仍然受载体线加速度影响。