一种基于磁强计和加速度计的快速对准方法与流程

本发明属于惯性导航技术领域，具体涉及一种基于磁强计和加速度计的快速对准方法。

背景技术：

惯性导航系统是一种不依赖于外部信息、也不向外部辐射能量的自主式导航系统。其工作环境不仅包括空中、地面，还可以在水下。惯导的基本工作原理是以牛顿力学定律为基础，通过测量载体在惯性参考系的加速度及角速度，将它们对时间进行积分，且把它们变换到导航坐标系中，就能够得到在导航坐标系中的速度、偏航角和位置等信息。

陀螺仪和加速度计是惯性导航系统中不可缺少的核心测量器件。高精度的惯性导航系统对所采用的陀螺仪和加速度计提出了很高的要求，而低精度惯性导航系统因惯性器件精度低无法采用陀螺自对准方法，一般采用磁强计和加速度计进行对准解算，并利用初始对准为惯性导航系统提供初始姿态航向。

在低精度惯性导航系统中，现有的对准方法一般采用磁强计和加速度计进行对准解算，并利用初始对准为惯性导航系统提供初始姿态航向，然而其对准精度和对准时间存在矛盾，缩短对准时间会降低对准精度，提高对准精度需要延长对准时间，因而其无法实现在保证对准精度的前提下减少对准时间。

技术实现要素：

针对现有技术的以上缺陷或改进需求，本发明提供了一种基于磁强计和加速度计的快速对准方法，其依据上一时刻的姿态矩阵以及三轴磁强计和三轴加速度计的测量值计算量测矩阵；依据上一时刻的姿态矩阵、当地地理坐标系下的地球磁场强度的反对称矩阵和地球重力加速度的反对称矩阵计算量测模型矩阵；采用迭代最小二乘估计或单次最小二乘估计计算旋转矢量；依据旋转矢量计算旋转矩阵和当前时刻的姿态矩阵；根据当前时刻的姿态矩阵解算出载体当前俯仰角、横滚角及航向角，从而在确保对准精度的前提下实现基于磁强计和加速度计的低精度惯性导航系统的快速对准。

为实现上述目的，按照本发明的一个方面，提供了一种基于磁强计和加速度计的快速对准方法，具体步骤为：

s1.依据上一时刻的姿态矩阵以及三轴磁强计和三轴加速度计的测量值计算量测矩阵；

s2.依据上一时刻的姿态矩阵、当地地理坐标系下的地球磁场强度的反对称矩阵和地球重力加速度的反对称矩阵计算量测模型矩阵；

s3.当前计算次数t大于预设的计算次数阈值n时，采用迭代最小二乘估计并利用量测矩阵和量测模型矩阵计算旋转矢量；否则采用单次最小二乘估计并利用量测矩阵和量测模型矩阵计算旋转矢量；

s4.依据旋转矢量计算旋转矩阵和当前时刻的姿态矩阵；

s5.根据当前时刻的姿态矩阵解算出载体当前俯仰角、横滚角及航向角，从而在确保对准精度的前提下实现基于磁强计和加速度计的低精度惯性导航系统的快速对准。

作为本发明的进一步改进，当前计算次数t的计算方法为：初始时刻设置为t＝0，间隔一个预设的采样周期计算次数t增加一次，从t＝1开始计算载体当前俯仰角、横滚角及航向角信息。

作为本发明的进一步改进，步骤s1具体为：

选择东北天当地地理坐标系作为导航坐标系，载体坐标系三个轴系定义为载体右-前-上，旋转顺序为3-1-2；

间隔一个预设的采样周期利用三轴磁强计和三轴加速度计采集当前时刻载体的磁强计信息m(t)和加速度计信息acc(t)；

计算载体坐标系下的地球磁场强度投影m^b(t)：

计算载体坐标系下的地球重力加速度投影acc^b(t)：

其中，b表示载体坐标系，n表示导航坐标系；其中为上一时刻姿态矩阵的转置矩阵，表示当前时刻的姿态矩阵，初始值设置为mⁿ为当地地球磁场强度；g为当地地球重力加速度；

依据磁强计信息m(t)、载体坐标系下的地球磁场强度投影m^b(t)、加速度计信息acc(t)和载体坐标系下的地球重力加速度投影acc^b(t)计算出量测矩阵z(t)：

作为本发明的进一步改进，步骤s2具体为：

当地地理坐标系下的地球磁场强度的反对称矩阵mⁿ(×)为：

当地地理坐标系下的地球重力加速度的反对称矩阵accⁿ(×)为：

依据上一时刻的姿态矩阵当地地理系下的地球磁场强度的反对称矩阵mⁿ(×)和地球重力加速度的反对称矩阵accⁿ(×)计算量测模型矩阵h(t)，

作为本发明的进一步改进，步骤s3具体为：

t＜n时，采用单次最小二乘估计计算姿态旋转矢量φ(t)为：

φ(t)＝[h^t(t)*h(t)]^-1*h^t(t)*z(t)

其中，h^t(t)为量测模型矩阵h(t)的转置矩阵；

t≥n时，采用迭代最小二乘估计计算旋转矢量φ(t)为：依据前一时刻的增益矩阵k(t-1)、姿态等效旋转矢量φ(t-1)和估计误差协方差阵p(t-1)依次计算当前时刻的增益矩阵k(t)、姿态等效旋转矢量φ(t)和估计误差协方差阵p(t)，表达式为：

k(t)＝p(t-1)*h^t(t)*[h(t)*p(t-1)*h^t(t)+r]-¹

φ(t)＝k(t)*[z(t)-k(t)*h(t)]*p(t-1)

其中，p(t)初始值设置为r表示量测噪声阵，其为对角线矩阵且其对角线的值分别为三轴磁强计的三个磁强计精度值和三轴加速度计的三个加速度计精度值。

作为本发明的进一步改进，步骤s4具体为：

旋转矩阵c1(t)为：

其中，|φ(t)|为旋转矢量φ(t)的模值；φ(t)×为旋转矢量φ(t)的反对称矩阵；

作为本发明的进一步改进，步骤s5具体为：

载体当前俯仰角为：

载体当前横滚角为：

载体当前航向角为：

其中，为当前时刻的姿态矩阵的第i行第j项。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，具有以下有益效果：

本发明的一种基于磁强计和加速度计的快速对准方法，其依据上一时刻的姿态矩阵以及三轴磁强计和三轴加速度计的测量值计算量测矩阵；依据上一时刻的姿态矩阵、当地地理坐标系下的地球磁场强度的反对称矩阵和地球重力加速度的反对称矩阵计算量测模型矩阵；采用迭代最小二乘估计或单次最小二乘估计计算旋转矢量；依据旋转矢量计算旋转矩阵和当前时刻的姿态矩阵；根据当前时刻的姿态矩阵解算出载体当前俯仰角、横滚角及航向角，从而在确保对准精度的前提下实现基于磁强计和加速度计的低精度惯性导航系统的快速对准。

本发明的一种基于磁强计和加速度计的快速对准方法，以三轴磁强计和三轴加速度计为基本数据传感器，依据不同计算周期分别采用单次最小二乘估计和迭代最小二乘估计计算姿态旋转矢量，确保实现姿态旋转矢量迭代算法的收敛速度和稳定性，从而能够快速获得载体姿态、航向信息，实现低精度惯性导航领域的快速对准。

本发明的一种基于磁强计和加速度计的快速对准方法，先用东北天当地地理坐标系为导航坐标系，通过计算载体坐标系下的地球磁场强度投影和重力加速度投影从而计算出量测矩阵，通过计算当地地理坐标系下的地球磁场强度投影和重力加速度投影从而计算出量测模型矩阵，从而进一步确保姿态旋转矢量迭代算法的实现，具有较高的工程化应用价值和推广价值。

附图说明

图1为本发明实施例的一种基于磁强计和加速度计的快速对准方法的示意图；

图2为本发明实施例的一种基于磁强计和加速度计的快速对准方法与传统对准方法的航向角对比示意图；

图3为本发明实施例的一种基于磁强计和加速度计的快速对准方法与传统对准方法的俯仰角对比示意图；

图4为本发明实施例的一种基于磁强计和加速度计的快速对准方法与传统对准方法的横滚角对比示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。下面结合具体实施方式对本发明进一步详细说明。

图1为本发明实施例的一种基于磁强计和加速度计的快速对准方法的示意图。如图1所示，该方法具体步骤如下：

s1.根据上一时刻的姿态矩阵以及三轴磁强计和三轴加速度计的测量值计算量测矩阵；

首先，选择东北天当地地理坐标系作为导航坐标系，载体坐标系三个轴系定义为载体右-前-上，旋转顺序为3-1-2。利用三轴磁强计和三轴加速度计采集当前时刻载体的磁强计信息m(t)和加速度计信息acc(t)，其中,t为当前迭代解算次数；

当前计算次数t的计算方法为：初始时刻设置为t＝0，间隔一个预设的采样周期计算次数t增加一次，从t＝1开始计算载体当前俯仰角、横滚角及航向角信息。

将地球磁场强度在当地地理坐标系下的投影转换为载体坐标系下的地球磁场强度投影m^b(t)，具体为：

其中，b表示载体坐标系，n表示导航坐标系；其中为上一时刻姿态矩阵的转置矩阵，初始值(t＝0)设置为mⁿ为当地地球磁场强度；

将地球重力加速度在当地地理坐标系下的投影转换为载体坐标系下的地球重力加速度投影acc^b(t)，具体为：

其中，为上一时刻姿态矩阵的转置矩阵，初始值(t＝0)设置为g为当地地球重力加速度；

(1d)依据磁强计信息m(t)、当地地理坐标系下的地球磁场强度投影m^b(t)、加速度计信息acc(t)和当地地理坐标系下的地球重力加速度投影acc^b(t)计算出量测矩阵z(t)，z(t)具体为：

s2.依据上一时刻的姿态矩阵、当地地理坐标系下的地球磁场强度的反对称矩阵和地球重力加速度的反对称矩阵计算量测模型矩阵；

当地地理坐标系下的地球磁场强度的反对称矩阵mⁿ(×)具体为：

当地地理坐标系下的地球重力加速度的反对称矩阵accⁿ(×)具体为：

依据上一时刻的姿态矩阵当地地理系下的地球磁场强度的反对称矩阵mⁿ(×)和地球重力加速度的反对称矩阵accⁿ(×)计算量测模型矩阵量测模型矩阵h(t)，具体为：

s3.当前计算次数t大于预设的计算次数阈值n时，采用迭代最小二乘估计并利用量测矩阵和量测模型矩阵计算旋转矢量；否则，采用单次最小二乘估计并利用量测矩阵和量测模型矩阵计算旋转矢量，具体为：

作为一个优选的实施例，预设的计算次数阈值为n＝10；

当t＜10时，采用单次最小二乘估计计算姿态旋转矢量φ(t)为：

φ(t)＝[h^t(t)*h(t)]^-1*h^t(t)*z(t)

其中，h^t(t)为量测模型矩阵h(t)的转置矩阵；

当t≥10时，采用迭代最小二乘估计计算旋转矢量φ(t)为：依据前一时刻的增益矩阵k(t-1)、姿态等效旋转矢量φ(t-1)和估计误差协方差阵p(t-1)依次计算当前时刻的增益矩阵k(t)、姿态等效旋转矢量φ(t)和估计误差协方差阵p(t)，

k(t)＝p(t-1)*h^t(t)*[h(t)*p(t-1)*h^t(t)+r]^-1

φ(t)＝k(t)*[z(t)-k(t)*h(t)]*p(t-1)

其中，p(t)初始值(t＝9)设置为量测噪声阵r阵位对角线矩阵，对角线的值分别为三个磁强计精度值和三个加速度计精度值。

单次最小二乘算法能够快速收敛但稳定性较差，而迭代最小二乘估计收敛速度慢，但稳定性好，因而依据不同计算周期分别采用单次最小二乘估计和迭代最小二乘估计计算姿态旋转矢量，确保实现迭代算法的收敛速度和稳定性，从而能够快速获得载体姿态、航向信息，实现低精度惯性导航在确保对准精度的前提下的快速对准。

s4.依据旋转矢量φ(t)计算旋转矩阵c1(t)和当前时刻姿态矩阵具体为：

旋转矩阵c1(t)为：

其中，|φ(t)|为旋转矢量φ(t)的模值；

当前时刻的姿态矩阵为为：

s5.根据当前时刻的姿态矩阵解算出载体当前俯仰角pitch(t)、横滚角roll(t)及航向角yaw(t)的信息，从而在确保对准精度的前提下实现基于磁强计和加速度计的低精度惯性导航系统的快速对准。具体为：

载体当前俯仰角为：

载体当前横滚角为：

载体当前航向角为：

其中，为当前时刻的姿态矩阵的第i行第j项。

图2、图3和图4分别为本发明实施例的一种基于磁强计和加速度计的快速对准方法与传统对准方法的航向角、俯仰角和横滚角对比示意图。如图2、3和4所示，航向角、俯仰角、横滚角的基准分别为90°、45°和45°，采用本发明所述的快速对准方法三个姿态在5s可收敛到航向角、俯仰角、横滚角的误差0.2°以内，而传统对准方法最大要20s才能收敛到航向角、俯仰角、横滚角的误差0.2°以内。其中，传统对准方法一般采用均值法，即在一段时间内计算基于磁强计和加速度计的传感器的均值，利用均值进行矢量计算获得对准结果。

综上所述，本发明提供的基于磁强计和加速度计的快速对准方法，以三轴磁强计和三轴加速度计为基本数据传感器，依据不同计算周期分别采用单次最小二乘估计和迭代最小二乘估计计算姿态旋转矢量，从而能够快速获得载体姿态、航向信息，对低精度惯性导航领域的快速对准技术有较大贡献。本发明简单巧妙，符合实际需求，进步显著且实用性强，能够大规模推广使用。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。