一种基于磁偏角、磁倾角的卡尔曼滤波导航方法与流程

本发明实施例涉及导航领域，尤其涉及一种基于磁偏角、磁倾角角的卡尔曼滤波导航方法。

背景技术

现代无人机普遍装有多种导航设备，例如惯性传感器imu、gps等，通常采用卡尔曼滤波进行数据融合和速度、位置的解算。但是，传统的组合导航中，卡尔曼滤波没有考虑到磁偏角以及磁倾角，而在工程实践中发现，如不考虑磁偏角和磁倾角的影响，则会导致在全球不同地区的导航效果出现较大差异。另外，在进行卡尔曼滤波ud序贯更新时，会出现对矩阵d对角线元素的除法计算，如果对角线元素之间的差异较大，可能会出现较大的计算误差。以上因素都大大制约了导航的精度和鲁棒性。

技术实现要素：

有鉴于此，本发明实施例提供了一种基于磁偏角、磁倾角的卡尔曼滤波导航方法，以解决现有技术中忽视磁偏角、磁倾角的影响所导致的导航精度低、移植性差的问题。

本发明实施例提供了一种基于磁偏角、磁倾角的卡尔曼滤波导航方法，所述方法包含以下步骤：

步骤一：设置载体坐标系，所述载体坐标系三轴表示为记作xyz；

步骤二：通过惯性传感器确定当前时刻的载体姿态，记作att＝[φθψ]，其中φ代表滚转角，θ代表俯仰角，ψ代表航向角；

步骤三：确定载体坐标系向导航坐标系的转换矩阵，记作

简化转换矩阵为以下形式

步骤四：采用四元数q＝[q0q1q2q3]，三个位置p＝[pnpepd]，三个速度v＝[vnvevd]，三轴加速度计零偏ba＝[baxbaybaz]，三轴陀螺零偏bω＝[bωxbωybωz]，一个磁偏角α，一个磁倾角γ这18个状态量作为系统状态，记作x＝[q0q1q2q3pnpepdvnvevdbaxbaybazbωxbωybωzαγ]^t；

步骤五：构建基于磁偏角、磁倾角的卡尔曼滤波；

步骤六：根据所述基于磁偏角、磁倾角的卡尔曼滤波进行载体速度、位置的解算。

优选地，所述载体坐标系与载体固联，坐标系符合右手法则，原点在载体重心处，x轴指向载体前进方向，y轴由原点指向载体右侧，z轴方向根据xy轴由右手法则确定；所述导航坐标系为ned导航坐标系。

优选地，设卡尔曼滤波系统的状态方程为：

其中，为系统状态的导数，f(x)为系统状态的函数，w为系统噪声，z为系统的量测状态，h(x)为系统的量测状态函数，v为量测噪声；

记上式具体化为：

其中，为机体向四元数的转换矩阵；

设量测数据中位置zp＝[zpnzpezpd]^t，速度为zv＝[zvnzvezvd]^t，机体的三轴磁场强度为zm＝[zmxzmyzmz]^t，be为地球磁场矢量和，其为一已知常量；

则系统的量测向量可以表示为

z＝[zpnzpezpdzvnzvezvdzmxzmyzmz]^t(6)

对f(x)和h(x)其求解雅克比矩阵得到矩阵f和h，可得系统线性化后的状态方程

优选地，所述卡尔曼滤波包括时间更新和量测修正；

所述时间更新的具体步骤包括：

其中，向量为系统状态向量的一步预测值，向量为上一时刻的估计值，矩阵φ为矩阵f的离散化形式，φk/k-1为一步转移矩阵，矩阵pk/k-1为状态向量的一步预测方差矩阵，矩阵pk/k-1为状态向量的上一时刻方差矩阵，矩阵qk为系统噪声方差阵；

所述量测修正的具体步骤包括：

其中，向量为系统状态向量的当前最优估算值，矩阵kk为增益矩阵，矩阵hk为量测矩阵，矩阵rk为量测噪声序列方差阵，矩阵i为单位矩阵。

优选地，在进行卡尔曼滤波时，为了消除数值误差的影响，对非负定方差矩阵坐如下分解：

设矩阵d为n阶方阵，其只有对角线元素为非0元素，通过使用归一化的方法，将矩阵d元素的数据限定在[0,1]中，防止因为数据尺度的不同造成发散，具体归一化方法如下：

此时矩阵d即完成了尺度变换。

本发明实施例提供了一种基于磁偏角、磁倾角的卡尔曼滤波导航方法，使用改进的卡尔曼滤波方法，充分考虑磁偏角和磁倾角的影响，具有较高鲁棒性和扩展性；并且针对ud序贯更新的卡尔曼滤波器，考虑到在进行ud序贯更新时，会出现对矩阵d对角线元素的除法计算，如果对角线元素之间的差异较大，可能会出现计算误差，因此对对角线元素进行尺度变换，计算完成之后，再变换回之前的尺度。所述方法具有运算量小，扩展方便，解算精度高等优点。

附图说明

图1是本发明实施例一提供的一种基于磁偏角、磁倾角的卡尔曼滤波导航方法的流程图；

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面结合附图对本发明具体实施例作进一步的详细描述。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。

另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部内容。在更加详细地讨论示例性实施例之前应当提到的是，一些示例性实施例被描述成作为流程图描绘的处理或方法。虽然流程图将各项操作(或步骤)描述成顺序的处理，但是其中的许多操作可以被并行地、并发地或者同时实施。此外，各项操作的顺序可以被重新安排。当其操作完成时所述处理可以被终止，但是还可以具有未包括在附图中的附加步骤。所述处理可以对应于方法、函数、规程、子例程、子程序等等。

实施例一

本发明实施例一具体可以应用在需要高精度导航的产品，如无人机定位，自动驾驶，数字城市，机器人导航等。图1为本发明实施例一提供的一种基于磁偏角、磁倾角的卡尔曼滤波导航方法的流程图。本实施例的方法具体包括：

110、设置载体坐标系，所述载体坐标系三轴表示为记作xyz。

在本市实例中，所述载体坐标系与载体固联，坐标系符合右手法则，原点在载体重心处，x轴指向载体前进方向，y轴由原点指向载体右侧，z轴方向根据xy轴由右手法则确定。

120、通过惯性传感器确定当前时刻的载体姿态，记作att＝[φθψ]，其中φ代表滚转角，θ代表俯仰角，ψ代表航向角。

130、确定载体坐标系向导航坐标系的转换矩阵，记作

简化转换矩阵为以下形式

140、构建基于磁偏角、磁倾角的卡尔曼滤波。

在本实施例中，采用四元数q＝[q0q1q2q3]，三个位置p＝[pnpepd]，三个速度v＝[vnvevd]，三轴加速度计零偏ba＝[baxbaybaz]，三轴陀螺零偏bω＝[bωxbωybωz]，一个磁偏角α，一个磁倾角γ这18个状态量作为系统状态，记作x＝[q0q1q2q3pnpepdvnvevdbaxbaybazbωxbωybωzαγ]^t。

设卡尔曼滤波系统的状态方程为：

其中，为系统状态的导数，f(x)为系统状态的函数，w为系统噪声，z为系统的量测状态，h(x)为系统的量测状态函数，v为量测噪声；

记上式具体化为：

其中，为机体向四元数的转换矩阵；

设量测数据中位置zp＝[zpnzpezpd]^t，速度为zv＝[zvnzvezvd]^t，机体的三轴磁场强度为zm＝[zmxzmyzmz]^t，be为地球磁场矢量和，其为一已知常量；

则系统的量测向量可以表示为

z＝[zpnzpezpdzvnzvezvdzmxzmyzmz]^t(6)

对f(x)和h(x)其求解雅克比矩阵得到矩阵f和h，可得系统线性化后的状态方程

150、根据所述基于磁偏角、磁倾角的卡尔曼滤波进行载体速度、位置的解算。

在本实施例中，所述卡尔曼滤波包括时间更新和量测修正；

所述时间更新的具体步骤包括：

其中，向量为系统状态向量的一步预测值，向量为上一时刻的估计值，矩阵φ为矩阵f的离散化形式，φk/k-1为一步转移矩阵，矩阵pk/k-1为状态向量的一步预测方差矩阵，矩阵pk/k-1为状态向量的上一时刻方差矩阵，矩阵qk为系统噪声方差阵；

所述量测修正的具体步骤包括：

其中，向量为系统状态向量的当前最优估算值，矩阵kk为增益矩阵，矩阵hk为量测矩阵，矩阵rk为量测噪声序列方差阵，矩阵i为单位矩阵。

优选地，在进行卡尔曼滤波时，为了消除数值误差的影响，对非负定方差矩阵坐如下分解：

设矩阵d为n阶方阵，其只有对角线元素为非0元素，通过使用归一化的方法，将矩阵d元素的数据限定在[0,1]中，防止因为数据尺度的不同造成发散，具体归一化方法如下：

此时矩阵d即完成了尺度变换。

本发明实施例提供了一种基于磁偏角、磁倾角的卡尔曼滤波导航方法，使用改进的卡尔曼滤波方法，充分考虑磁偏角和磁倾角的影响，具有较高鲁棒性和扩展性；并且针对ud序贯更新的卡尔曼滤波器，考虑到在进行ud序贯更新时，会出现对矩阵d对角线元素的除法计算，如果对角线元素之间的差异较大，可能会出现计算误差，因此对对角线元素进行尺度变换，计算完成之后，再变换回之前的尺度。所述方法具有运算量小，扩展方便，解算精度高等优点。

在此请注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。