用于低成本的MARG传感器进行姿态估计的快速互补滤波方法与流程

本发明属于快速互补滤波器领域，特别涉及用于低成本的marg传感器进行姿态估计的快速互补滤波方法。

背景技术：

1、marg传感器由mems三轴陀螺仪、三轴加速度计和三轴磁力计组成，它已经被广泛应用于人体运动跟踪、无人机自主飞行、步行导航等需要确定姿态的领域。与传统的惯性传感器如光纤陀螺、环式激光陀螺等不同，mems惯性传感器存在着较大的误差，如非线性、随机游走、温度漂移等，这意味着不能仅仅依靠单个mems惯性传感器来获得准确的姿态估计。为了确保一个可靠的姿态解决方案，marg传感器必须使用传感器融合算法。在过去的几十年里，传感器融合已经得到了很好的发展，产生了许多很有价值的算法，例如互补滤波算法和卡尔曼滤波算法。

2、现在已有的marg传感器融合算法主要可以分为两类：一类涉及互补滤波器，另一类涉及卡尔曼滤波。互补滤波器充分利用陀螺仪在短时间内的准确性，以及加速度计可以通过重力确定姿态的特性，耗费较小的计算资源就可以实现较好的姿态测量结果。卡尔曼滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。互补滤波器相对简单，易于应用，卡尔曼滤波器需要更复杂的矩阵操作，很难在低硬件配置的平台上实现。

3、现有的融合算法大多构建了一个非线性的系统，采用迭代的方法，例如：高斯牛顿法，列文伯格-马尔夸特法求解，这不仅需要耗费更多的计算资源，而且需要更大的内存来保存计算数据。另外，现有的融合算法在有较大的外部加速度或有较大的磁干扰的情况下，估计出的姿态准确性会较差。

4、目前已经申请技术如下：

5、申请号：cncn202110163595.2，申请名称：一种基于自适应参数互补滤波的机器人姿态融合方法，其包括如下步骤：a1、对加速度值进行预处理并选取频率在1-100hz的分量，依据滤波后的加速度计算加速度和磁力计的置信度；a2、计算加速度与标准重力向量的误差以及磁力计与ned坐标系下的方向向量误差；a3、结合置信度和误差向量对陀螺仪测量值进行矫正；a4、结合四元数微分方程对姿态向量进行更新。本发明方法基于置信度函数动态修正加速度和磁力计传感器融合权重，有效提升机器人imu在大机动运动时姿态计算精度和鲁棒性。

6、其采用的方案是，先对加速度计进行了滤波，选取频率在1-100hz的分量，并需要计算加速度和陀螺仪的置信度，在进行多传感器融合时，基于置信度函数动态修正加速度和磁力计传感器融合权重。使用低通滤波器，需要计算频域分量，计算频域分量需要较大的算力，难以在低硬件配置的平台上实现。而我们采用的方案是，先对加速度计数据计算得到的姿态和陀螺仪数据计算得到的姿态进行互补滤波，再将滤波后的结果与结合磁力计数据计算得到的结果进行第二次互补滤波。我们的方法需要的算力小，可以部署在低算力的嵌入式平台上。

7、申请号：cn201810285907.5，申请名称：一种基于改进四元数二阶互补滤波的飞行器姿态解算方法，属于数字滤波和多传感器数据融合技术领域，主要作用于飞行器姿态解算系统中，使之获得准确的姿态角便于进行后续姿态控制。该方法采用加速度计和磁力计的误差值经过pi调节后对陀螺仪进行补偿和校正，并在飞行器的启动阶段自适应的调节滤波器截止频率，用补偿后的陀螺仪数据去更新四元数并归一化，再用更新后的四元数计算出姿态变换矩阵并解算出姿态角。本发明不仅具有传统互补滤波不需对噪声精确建模的优点，而且飞行器初始姿态收敛速度快，获得精确姿态角，同时减少滤波算法计算量，实现飞行器的快速平稳启动，增强其稳定性能。

8、其采用加速度计和磁力计的误差值经过pi调节后对陀螺仪进行补偿和校正，在飞行器的启动阶段自适应的调节滤波器截止频率，用补偿后的陀螺仪数据去更新四元数并归一化，再用更新后的四元数计算出姿态变换矩阵并解算出姿态角。而我们的专利中并没有对陀螺仪进行补偿和校正，而是使用陀螺仪对加速度计数据计算得出的姿态进行校正，之后并没有直接更新四元数得到姿态，而是进过了互补滤波器融合了磁力计数据。我们的方法充分利用了加速度计可以测得重力的特性，在准静态条件下可以得到更准确的姿态估计值，再融合陀螺仪和磁力计的测量值，可以有效提高运动中姿态估计的准确性。

9、申请号：cn201611063441.1，申请名称：一种基于线性卡尔曼滤波的航姿信息融合方法，其步骤如下：建立低精度惯导误差方程、建立失准角与水平加速度间关系以及线性卡尔曼滤波器模型，接着使用陀螺测量值进行姿态更新、把加速度测量值转换到导航系、计算加速度误差以及用估算的失准角修正姿态四元数，实现实时估计-修正，最后转换成欧拉角显示。该方法解决了ekf算法计算量大和状态向量偏导数难以解析求取的问题，同时又可以实时调节比例系数和估计陀螺仪零偏；对陀螺仪建立的一阶马尔科夫模型，避免滤波器过渡收敛的问题；在性能上优于互补滤波和梯度下降算法，与ekf算法性能相当，但在计算量和变成复杂度上又小于ekf算法，是一种比较适合工程实现的方法。

10、其采用了基于卡尔曼滤波的方案，而本技术采用的是基于互补滤波的方案，相比于卡尔曼滤波方案，本技术具有计算量小的特点，可以部署在低算力平台。

11、申请号：cn202111644288.2，申请名称：一种基于自适应互补滤波的九轴磁罗盘数据融合方法，步骤如下：利用九轴磁罗盘中三轴磁传感器数据以及三轴加速度计数据计算系统状态初值；通过三轴磁场数据判断载体周围是否存在磁干扰，如果存在磁干扰，则根据三轴测得的三轴磁异常数据实时解算出磁干扰目标源的实时位置，实现磁干扰补偿；利用磁场和加速度构建误差向量；模糊控制求解互补滤波的pi参数；利用互补滤波实时解算得到的姿态四元数求解载体姿态角。本发明能够通过自适应互补滤波数据融合实现高精度姿态角输出，且在载体周围存在磁干扰时，可实时解算出磁干扰目标源的实时位置来进行磁干扰补偿，最终输出的姿态角不受运动加速度及外界磁干扰的影响，且误差不会随着时间的增长而累积。

12、该申请利用三轴磁传感器数据以及三轴加速度计数据计算系统状态初值。而我们只使用三轴加速度计估计系统的初始姿态。磁传感器容易受到其他磁场源的干扰，从而给出错误的估计，而利用加速度计在静态或准静态情况下估计的姿态较为准确，所以我们只使用加速度计估计初值，在之后的流程中再引入磁传感器。

13、且其利用磁传感器解算出磁干扰目标源的实时位置，实现磁干扰补偿，再与加速度计组合，没有使用陀螺仪。而我们融合了陀螺仪的输出，能够拥有更高的精度。

技术实现思路

1、为了解决以上问题，本技术提出用于低成本的marg传感器进行姿态估计的快速互补滤波方法，针对姿态四元数计算过程中的非线性问题，将基于加速度计的姿态估计问题转化为一个线性问题。为了提高marg姿态估计的准确性，设计了一种新的互补滤波器结构，它的特征是首先通过融合加速度计和陀螺仪的测量来估计重力。同时，在算法中引入了磁失真检测，磁力计的输出与基于markley算法估计的重力相融合。

2、为实现上述目的，本发明采取的技术方案是：

3、本发明提供用于低成本的marg传感器进行姿态估计的快速互补滤波方法，具体步骤如下：

4、步骤1)利用加速度计输出的数据估计姿态四元数；

5、步骤2)使用陀螺仪进行补偿的重力滤波器；

6、步骤3)重力与磁力计的融合。

7、作为本发明进一步改进，所述步骤1)利用加速度计输出的数据估计姿态四元数；

8、具体步骤如下：

9、加速度计的旋转方程表示如下：

10、ab＝car    (1)

11、其中，ar是加速度计测量值在东北天坐标系下的表示，ab是加速度计测量值在载体坐标系下的表示，c是旋转矩阵；

12、旋转矩阵表示为以下形式：

13、c＝(c1,c2,c3)    (2)

14、其中，

15、

16、

17、

18、其中，q0，q1，q2，q3为旋转矩阵对应的四元数；

19、将式(1)进行分解，得到：

20、

21、其中，qa表示从加速度中得到的四元数，这个等式被转换为：

22、

23、其中，代表穆尔-彭罗斯广义逆矩阵；

24、利用式(7)变为：

25、

26、定义wa为：

27、

28、则式(8)变为：

29、waqa＝qa    (10)

30、可以很容易的看出，对于任意选择的q0满足式(10)；

31、式(10)被看做q的一个迭代方程：

32、qa(t)＝waqa(t-1)    (11)

33、需要注意的是：

34、

35、式(11)被看作是一个线性离散常系数动力学系统，被转化为一个连续系统，根据现代控制理论，一个线性离散常系数系统被转化为：

36、

37、其中，

38、

39、其中，t代表采样间隔，由于在wa中有零项，wa的自然对数可能包含-∞项，，因此，为了近似得到ha，使用的一阶泰勒展开：

40、wa＝i4×4+hat    (15)

41、这样就产生了ha的近似值：

42、

43、其中，时间间隔t被选择的非常小，因此式(11)式的连续形式变为：

44、

45、这个连续系统的解推导出为：

46、

47、其中，指数部分被计算为：

48、

49、其中，

50、

51、通过这种方式，被重写为：

52、

53、设定

54、

55、这样得到：

56、

57、因此：

58、

59、当t→+∞，式(24)变成了：

60、

61、给定任意q0，解通过下式给出：

62、

63、当q0＝(1,0,0,0)t，式(26)变为：

64、

65、归一化后，得到：

66、

67、即来自加速度计的四元数可以立即用式(28)计算出来；

68、测量误差通过下式给出：

69、

70、四元数增量由下式给出：

71、

72、作为本发明进一步改进，所述步骤2)使用陀螺仪进行补偿的重力滤波器具体步骤如下：t时刻重力在载体坐标系下的投影使用以下公式计算：

73、

74、其中表示通过加速度计和陀螺仪得到的融合方向的速率，通过下式定义：

75、

76、其中，γ表示互补滤波的增益，式(32)进一步表示为：

77、

78、对得到的四元数进行归一化：

79、

80、在更新期间，利用四元数qest,aω,t估计得到重力

81、

82、作为本发明进一步改进，所述步骤3)重力与磁力计的融合具体步骤如下：

83、四元数qopt的封闭形式表示为：

84、

85、其中参数为：

86、

87、

88、

89、其中表示一次向量观测的权值。

90、在这种情况下，向量b1，r1，b2，r2，r×和b×被定义为：

91、

92、其中，gb代表经过互补滤波器补偿过的重力向量，利用从传感器融合中提取的信息得到：

93、

94、然而，由于磁力计的噪声，算得的四元数不够平滑，在这种情况下，应该采用一个滤波器来平滑估计值，让重力-磁力四元数为qgm，最终姿态估计器产生qamω，那么基于磁力计的四元数通过以下方式进行过滤：

95、

96、其中，γm代表滤波器增益，对结果归一化产生最终姿态的估计：

97、

98、

99、本发明用于低成本的marg传感器进行姿态估计的快速互补滤波方法,设计要点如下；

100、1、将基于加速度计的姿态估计问题转化为一个线性问题，将得到的稳定解用作互补滤波器的测量；

101、2、设计了一种新的互补滤波器结构，特征是首先通过融合加速度计和陀螺仪的测量来估计重力；

102、3、引入了磁失真检测，磁力计的输出与基于markley算法估计的重力相融合。

103、本发明针对基于marg传感器的姿态融合算法中，四元数求解时的非线性问题，将基于加速度计的姿态估计问题转化为一个线性问题，得到的稳定解用作互补滤波器的测量量。另外，为了提高融合算法的精度，设计了一种新的互补滤波器结构，它的特征是首先通过融合加速度计和陀螺仪的测量来估计重力，同时引入了磁失真检测，磁力计的输出与基于markley算法估计的重力相融合。