一种基于扩展卡尔曼滤波的姿态解算方法与流程

本发明属于导航，尤其涉及惯性导航领域，具体为一种基于扩展卡尔曼滤波的姿态解算方法。

背景技术：

1、捷联式惯性导航系统 (strapdown inertial navigation system)是把惯性器件直接固连在载体上，用计算机来完成导航平台功能的惯性导航系统；其有点包括：1.整个系统的体积、重量和成本大大降低；2.惯性仪表便于安装维护，便于更换；3.惯性器件可以给出轴向的线加速度和角速度，这些信息是控制系统所需要的。和平台式系统相比，捷联式系统可以提供更多的导航和制导信息；4.惯性器件便于采用余度配置，提高系统的性能和可靠性。

2、三轴陀螺仪和三轴加速度计（惯性传感器）与三轴磁强计组成9轴惯性单元，9轴惯性单元与载体“捆绑”，载体的姿态和航向可以用载体坐标系（b系）相对于导航坐标系（n系）的三个转动角决定，就是说，在捷联式惯性导航系统中，直接测得的三个方向的加速度是载体坐标系三个轴方向上的，需要用一个捷联矩阵c将其变换到导航坐标系中来。惯性传感器安装在载体；该形式的捷联式惯性导航系统的任务是解算出载体相对于当地水平坐标系的位姿，速度和位置，其中，最为关键的任务是位姿的解算。

3、三轴磁力计、三轴加速度计和三轴陀螺仪的测量值可以结合起来，给出三维的方向估计。为了获得准确的方向估计，必须对磁强计和惯性传感器（加速度计和陀螺仪）进行轴对齐，并对磁强计的传感器误差和磁畸变进行校正。

技术实现思路

1、（一）要解决的技术问题

2、为解决现有技术的上述问题，本发明提供一种基于扩展卡尔曼滤波的姿态解算方法，可以有效解决：

3、（1）传统的姿态估计方法需要针对不同的传感器类型将磁强计误差和畸变矫正与姿态解算方法分开进行，增加了算法部署的步骤，另一方面，传统的磁强计的校准磁强计只考虑了传感器误差和由磁强计安装引起的磁扰动，没有考虑到与惯性传感器的轴对齐问题，进而造成位姿估计不准确；

4、（2）传统基于四元数的位姿估计由于惯性测量单元的特点使误差逐渐增大，使估计结果越来越不可靠；

5、（3）传统使用扩展卡尔曼滤波的姿态估计方法需实时计算过程噪声矩阵使计算量增大，对于实时性要求较高的应用场景，该方法就显得差强人意，基于传统四元数的扩展卡尔曼姿态估计在实践中的表现并不理想。

6、（二）技术方案

7、为了解决上述问题，本发明采用的一种技术方案为：一种基于扩展卡尔曼滤波的姿态解算方法，包括在线磁强计校正模块、状态变量确定模块、外推方程构建模块，加速度计与磁力计估计模块、姿态偏置状态外推模块和姿态偏置状态更新模块；在线磁强计校正模块的作用是消除原始磁强计数据的零偏误差、交轴耦合误差和比例误差，并将磁强计的三个轴与加速度计三个轴进行轴向对齐，为实现此功能，在线磁强计校正模块的操作步骤如下：

8、s1、建立磁强计测量模型：

9、一、设使用磁强计进行测量的测量数据集为：，上标 b表示磁强计的传感器坐标系（也称body系）， k表示第 k个采样数据，上标 n表示数据集中的样本总数，每个样本包含三个分量，分别是磁强计x轴的读数，磁强计y轴的读数和磁强计z轴的读数；当地磁场矢量从 n系（当地水平坐标系）到b系（传感器坐标系）的转化用旋转矩阵用表示，对第 k个采样数据， n系的当地磁场转换到b系中来的公式为：

10、

11、上标 n表示导航坐标系（ n系也叫当地水平坐标系，采用北东地ned）

12、进而有：

13、

14、上式是对每一个样本点根据当地磁场和对应的旋转矩阵用估计的结果，我们希望实际测量值与估计值相等，也就是成立，然而，有两个原因导致它们在实践中通常不平等，需要校准；磁力计三个轴的传感器误差：每个磁力计的轴向误差不同。这些传感器误差包括：

15、（1）磁强计三个轴的交轴耦合，使用3×3的矩阵 n表示磁强计三个轴的交轴耦合；

16、（2）磁强计零偏置，即使磁场为零，磁强计测量结果为非零，零偏置由3×1向量 b定义；

17、（3）磁强计三个轴的比例因子不同，用3×3对角矩阵 sm表示；

18、（4）在磁强计的测量中存在高斯噪声，用3×1向量表示；

19、这导致了和之间的以下关系，只考虑传感器误差：

20、

21、二、考虑硬铁效应和软铁效应：硬铁效应是由于磁性材料的永久磁化强度，会导致一个恒定的附加磁场，表示这个效应的向量记为 h；软铁效应是由于外部磁场对材料的磁化作用，因此将取决于材料相对于外部磁场的方向，它可以改变被测磁场的大小和方向，表示软铁效应的3×3矩阵记为 s i；假设软铁产生的场与外部磁场之间存在线性关系，这是一个合理的假设，因为磁滞只有在施加非常大的磁场时才会发生；在考虑了硬铁效应和软铁效应后，传感器误差模型变为：

22、

23、假设满足这些要求，式（1）意味着 n系中的投影到 b系中的将位于一个半径等于局部磁场大小的球体上。对式（3），假设所有的样本的数据应该位于一个椭球体上，这个椭球体可以相对于上述球体进行平移、旋转和缩放。

24、三、考虑磁力计和惯性传感器轴不对齐：现在考虑磁强计三轴与惯性传感器的轴不对齐问题造成位姿估计不准确，使用旋转矩阵 r im描述惯性传感器和磁力计之间的错位关系，式（4）变为：

25、

26、其中：

27、

28、最终的校正模型为：

29、

30、上式中， d为失真矩阵， q为偏移向量，意味着为了获得正确的校准，不需要在式（5）中区分交轴耦合、零偏置、比例因子、硬铁效应、软铁效应和磁力计和惯性传感器轴不对齐，它们可以组合成一个3×3失真矩阵 d和一个3×1偏移向量 q；

31、s2、最大似然建模：

32、一、磁强计的原始数据通过将椭球体数据映射到球体来补偿传感器误差和磁畸变的存在；然而，磁力计和惯性传感器轴不对齐不能仅仅通过磁强计的测量来校正；将磁强计测量和惯性测量相结合以确定这种旋转是必要的，椭球的旋转可以通过使用磁强计测量和处理过的垂向参考值来确定，其中第 k个样本表示为：

33、

34、其中：

35、

36、式（8）中，服从高斯分布， z n为单位垂向向量，处理后的测量值可以通过保持加速度计在不同方向上的静止来得到，且模长也为1；

37、二、当地地球磁场建模：

38、当地地球磁场由一个水平分量和一个垂直分量组成，设地球磁场与水平面成δ角,由sin δ定义的垂直分量，由cos δ定义的水平分量，

39、

40、上式中，表示的模长，和水平面之间的夹角δ取决于在地球上的位置， d表示垂向分量，由于公式（7）中的校准结果 d和 q只依赖于水平分量和垂直分量之间的比例，因此可以假设为零；

41、从公式（10）和的定义中可以看出，和的内积为的垂直分量 d；因此，和的内积对于所有可能的旋转上也等于 d；然而，当惯性和磁力计传感器轴不对齐时，基于磁力计和垂直测量的每个的估计 d将会有所不同，选择一种旋转 r im可使惯性和磁强计传感器轴对齐，而使估计的倾角 d近似恒定；

42、s3、构建最大似然公式：磁强计测量值在校准前位于一个椭球体上，并在校准后位于一个半径为||||的球体上，将球体的半径缩放为1，即||mn || = 1；

43、结合式（7）和式（1）得：

44、

45、定义参数 θ1为：

46、

47、如前所述，上式中， d表示失真矩阵， q表示偏移向量，是第二个分量为0的当地磁场矢量，表示所有样本点从 n系到 b系的旋转矩阵；假设噪声是独立的和高斯分布的，有：

48、

49、其中和为对角矩阵：

50、定义参数 θ2为：

51、

52、s4、求解极大似然问题，也就是使和尽可能相等，从而找到未知参数 θ1和 θ2的估计值，有：

53、

54、 y1:k定义为：

55、

56、上式中，上标 m表示估计值，上标 z表示测量值，由于噪声服从独立的高斯分布，则将ml问题简化为：

57、

58、使用三个分量参数化旋转矩阵和mn时，假设 σm和 σz是对角矩阵，求解给定 θ1的 θ2是通过先求解假设 θ2为恒等式的 θ1，然后求解给定 θ1的 θ2，最后求解给定 θ2的 θ1，而不是一次性求解式（14）。

59、进一步地，所述状态变量确定模块的处理步骤如下：

60、s1、定义状态变量：

61、

62、上式中， q 0  是四元数的实部， q 1， q 2， q 3分别是四元数虚部的三个分量； b 1， b 2， b 3  分别是陀螺仪在x，y，z 三个方向的偏置；偏置指的是陀螺仪在单位时间内的漂移量，可将式（15）写成如下形式：

63、

64、上式中， q表示姿态四元数， b表示偏置向量。

65、s2、定义四元数求导法则：

66、

67、其中： s(ω)、 q、 s(q)、 ω的表达式分别为：

68、

69、

70、s3、为了补偿陀螺仪的偏差，在方程中加入偏差项：

71、

72、s4、使用一阶线性化模型将上述方程进行离散化：

73、

74、上式中， t是 k时刻到 k+1时刻的采样时间， q( k)是 k时刻的姿态四元数， q( k+1)是 k+1时刻的姿态四元数，是k时刻的姿态四元数关于时间的导数；

75、

76、将式（18）代入式（19）中得：

77、

78、

79、因此，系统状态方程可以写成：

80、

81、也可以写成：

82、

83、上式可简写为：

84、

85、矩阵 a为状态转移矩阵，矩阵 b为输入转移矩阵，为k时刻的状态变量，为k时刻的输入向量，为外推的状态向量；上式就是扩展卡尔曼滤波中的状态外推方程。

86、进一步地，所述加速度计与磁力计估计模块包括加速度估计和磁场估计两个步骤：

87、s1、首先其中加速度估计将重力矢量作为参考矢量，重力矢量总是指向当地水平坐标系的下方，有：

88、

89、上式中，是加速度计在传感器坐标系（ b系）下估计的测量加速度；是当地水平坐标系到传感器坐标系的旋转矩阵； g是当地水平坐标系下的重力向量， g = [0 0 1]t；是传感器坐标系下的加速度计零偏，当对加速度计做了校正后是0；因为可以确定等号右边的所有变量，因此可以估计加速度计的测量值，然后使用加速度计的预测测量值与实际测量值进行比较，以确定方向误差，从单位四元数到旋转矩阵的定义如下：

90、

91、由于重力向量为 g = [0 0 1]t；因此有：

92、

93、上式中，含有四元数的矩阵是非线性的，因此，为使用卡尔曼滤波，将上式进行线性化，并写成形如的形式，是状态向量，是余项，扩展卡尔曼滤波的处理方式是将上式进行线性化，根据泰勒展开，有：

94、

95、其中为雅可比矩阵：

96、

97、

98、将上式写成的形式， ca称为线性系数矩阵，并忽略矩阵有：

99、

100、s2、其次，磁场估计的参考矢量是地球磁场提供的一个始终指向磁北方向的矢量，这个矢量在地球表面上的变化(矢量的高度取决于海拔) ，假设北参考矢量的变化是可以忽略不计的，因为传感器不会移动太多(只在房间内)；假设磁强计数据在水平面上提供的参考矢量是准确的，因此要从磁强计参考矢量中去掉1维；

101、

102、上式中， h表示真实的磁场， hm表示磁强计测得的磁场； d-1表示失真矩阵的逆矩阵， q是偏移向量，都是在线磁强计校正模块的处理结果；接下来，为了从磁强计矢量中移除1维(垂直平面) ，由于想移除的磁场垂直分量存在于当地水平坐标系中，因此首先将坐标从传感器坐标系转换为当地水平坐标系，因此，由四元数生成旋转矩阵，如下所示：

103、

104、上式中，

105、，

106、上式中， r的每个分量是传感器坐标系下校正之后的磁强计数据， r’的每个分量是在当地水平坐标系下校正之后的磁强计数据，令 r’的第二个分量等于零，然后重新标准化向量，使它保持为一个单位向量，然后将旋转它回到传感器坐标系，并使用结果向量代替实际校准的测量数据。

107、使用北矢量作为磁强计的参考，因为北矢量恰好指向x轴的正方向，所以只是把正x轴(当地水平坐标系)作为参考矢量；

108、

109、其中，是磁强计在传感器坐标系下的估计的磁场强度值，是当地水平坐标系到传感器坐标系的旋转矩阵，是当地水平坐标系下参考磁场的北向分量，且有=[1 00 ]t；是传感器坐标系下的磁强计零偏值，当对磁强计进行校正后，为零，旋转矩阵的表达式为：

110、

111、化简得：

112、

113、上式依然是一个非线性方程，利用泰勒展开可以化简为：

114、

115、因此，线性化之后的表达式为：

116、

117、进一步地，所述姿态偏置状态外推模块的处理过程如下：

118、s1、进行状态外推：；在状态外推中，表示由 k-1时刻的四元数生成的状态转移矩阵、是 k-1时刻的状态变量、表示由 k-1时刻的四元数生成的输入转移矩阵、为 k时刻输入，也就是 k时刻的陀螺仪的测量值，是外推结果，也就是外推得到的状态变量；

119、s2、进行协方差外推：；在协方差外推中， q为过程噪声协方差矩阵，是 k - 1时刻的状态误差协方差矩阵，表示由 k-1时刻的四元数生成的状态转移矩阵；表示外推得到的 k时刻的状态误差协方差矩阵。

120、进一步地，所述姿态偏置状态更新模块的处理过程如下：

121、s1、计算卡尔曼增益矩阵：

122、上式中 r是测量协方差矩阵， c是线性系数矩阵（也称为测量矩阵），表示外推得到的 k时刻的状态误差协方差矩阵， k k为卡尔曼增益矩阵；

123、s2、进行状态更新：

124、上式中是外推得到的状态变量， c是线性系数矩阵（也称为测量矩阵）， k k为卡尔曼增益矩阵，是 k时刻的加速度计测量和磁强计测量的组合向量，是更新得到的状态变量；

125、s3、进行协方差更新：

126、上式中， i是7×7的单位矩阵， k k为卡尔曼增益矩阵，表示外推得到的 k时刻的状态误差协方差矩阵， c是线性系数矩阵， r k是测量误差协方差矩阵，是更新后的状态误差协方差矩阵。

127、（三）有益效果

128、本发明提供一种基于扩展卡尔曼滤波的姿态解算方法，可有效解决：

129、（1）将磁强计误差和畸变矫正与姿态解算方法同时进行，考虑了传感器误差和由磁强计安装引起的磁扰动，并考虑到了与惯性传感器的轴对齐问题，进而使位姿估计更加准确；

130、（2）使用扩展卡尔曼滤波，使估计结果越来越可靠；

131、（3）将过程噪声协方差矩阵和测量误差协方差矩阵设为常值矩阵，在满足实时性要求的同时获得精度较高的结果。