CN104121905A - 一种基于惯性传感器的航向角获取方法 - Google Patents

Abstract

本发明公开了一种基于惯性传感器的航向角获取方法，首先使用陀螺仪和磁力计的测量值单独计算航向角，然后对这两种结果加权融合。在利用陀螺仪求航向角时，对陀螺仪测得的载体坐标系下z轴方向角速度进行积分，根据当前时刻的总角速度值与阈值的大小关系判断是否进行积分；在利用磁力计计算航向角时，将地理坐标系的水平面划分为8个象限，并将磁力计测得的磁场强度值投影到该水平面上，根据水平面轴与轴方向上的磁场强度分量对应到具体的象限，在每个象限采用不同的修正因子对现有的航向角计算公式进行修正。最后，利用角速度的方差值设置加权系数，对这两种方法求得的航向角进行融合。本发明能够获得更加准确的航向角值，从而使定位的精度更高。

Description

一种基于惯性传感器的航向角获取方法

技术领域

本发明属于无线定位技术领域，特别涉及一种基于惯性传感器的航向角获取方法。

背景技术

近年来数据业务的快速发展使人们对定位服务的需求不断增长。目前应用比较广泛的卫星导航系统几乎能够做到全球覆盖，然而，由于卫星导航系统的信号穿透能力较弱，在城市峡谷以及室内等复杂的环境中，其信号能量和接受信噪比都比普通环境中有很大程度的恶化，信号的可用性和总体性能都大大下降，接收机定位精度很差，甚至无法定位。因而如何在室内提供一种比较精确的定位方法成为目前比较急迫的需求之一。

惯性导航技术作为应运而生的一种室内定位技术，其结合行人航迹推算(PDR，Pedestrian Dead Reckoning)算法,经过步伐检测、步长估计、航向角计算三个步骤，能够对室内行人进行比较准确的定位。其中，航向角一般可以通过陀螺仪测得的角速度或者磁力计测得的磁场强度经过计算得到，使用陀螺仪进行角速度积分求航向角，短时间内能够取得比较准确的结果，但随着时间的推移会存在比较大的累积误差；使用磁力计求航向角，会存在硬件自身固有偏差，以及测量得到的磁场强度不稳定等问题，这些都会导致航向角的误差，从而影响定位的精度。

发明内容

发明目的：本发明为了克服现有技术中存在的不足，本发明提出一种能够有效提高获取到的航向角的准确度的基于惯性传感器的航向角获取方法

发明内容：为解决上述技术问题，本发明提供了一种基于惯性传感器的航向角获取方法，包括以下步骤：

步骤10：采用陀螺仪测量载体坐标系三轴向上的角速度；根据测得的角速度获取此刻的载体的航向角；

步骤20：采用磁力计测量载体所在的磁场强度值，根据测得的磁场强度值获取此刻的载体的航向角；

步骤30：将步骤10和步骤20获得的航向角进行加权融合获得最后的航向角。

进一步，所述步骤10中获得载体的航向角的方法为：

步骤101：设定总角速度阀值W＝m·g，其中，m＝0.001～0.002，g为重力加速度；

步骤102：将陀螺仪测得的载体坐标系三个轴向上的角速度值求取总角速度值，将获得的总角速度值与步骤101中设定的总角速度阀值进行比较，如果总角速度值大于总角速度阀值，对载体坐标系z轴方向上的角速度值进行时间上的积分得到此时载体的航向角。

进一步，所述步骤20中获得载体的航向角的方法为：

步骤201：将地理坐标系水平面划分为8个象限；

步骤202：在每个象限中等间隔选取n个角度，n个角度值为将磁力计水平摆放朝向这n个角度，分别测量在不同角度时，水平面上x轴与y轴方向上的磁场强度值h_rx和h_ry；

步骤203：根据公式计算n个角度真实值计算n个角度值与真实值之间的差值为

步骤204：根据公式计算第d个象限的修正因子Q_d，其中，q_k为第k个角度的角度值与真实值之间的差值；

步骤205：在每个象限中分别采用每个象限对应的修正因子对航向角进行修正。

进一步，所述步骤30中通过加权融合获得最后的航向角的方法为：

步骤301：设定长度为的滑动窗口，其中，T为陀螺仪的数据采样时间间隔，以秒为单位；

步骤302：将陀螺仪每次测量得到新的载体坐标系z轴方向的角速度值w_zi时，将最新测量得到的值加入到窗口的最末端，并将窗口内最先得到的值移除；

步骤303：计算求得此时窗口内所有角速度值的方差为w_var，利用所得方差值设置加权系数c＝5，k_gyr为陀螺仪测量得到的航向角的加权系数，k_com为磁力计测量得到的航向角的加权系数；

步骤304：根据公式获得最终的航向角，其中，为最终的航向角，为陀螺仪测量得到的航向角，为磁力计测量得到的航向角。

工作原理：本发明首先使用陀螺仪和磁力计，即为罗盘的测量值单独计算航向角，然后对这两种结果加权融合。在利用陀螺仪求航向角时，对陀螺仪测得的载体坐标系下z轴方向角速度进行积分，根据当前时刻的总角速度值与阈值的大小关系判断是否进行积分；在利用磁力计计算航向角时，将地理坐标系的水平面划分为8个象限，并将磁力计测得的磁场强度值投影到该水平面上，根据水平面x轴与y轴方向上的磁场强度分量对应到具体的象限，在每个象限采用不同的修正因子对现有的航向角计算公式进行修正。最后，利用角速度的方差值设置加权系数，对这两种方法求得的航向角进行融合。

有益效果：与现有技术相比，本发明充分考虑使用陀螺仪和磁力计计算航向角时存在的不同问题，针对性地提出相应的修正方案，对两种计算方法进行单独的改进和完善。在此基础上，结合在动态运动时，陀螺仪得到的航向角比较准确，而在静止或缓慢运动时，磁力计得出的航向角相对准确的特点，将行人行走时角速度方差值的相关函数作为加权系数，对上述两种结果进行加权融合，能够获得更加准确的航向角值，从而使定位的精度更高。

附图说明

图1为使用陀螺仪获取航向角的流程图；

图2为使用磁力计计算航向角的流程图；

图3为水平面8象限划分图；

图4为使用融合算法求航向角值的流程图；

图5为采用本发明提供的航向角计算方法与仅使用陀螺仪或磁力计计算航向角的定位结果比较图。

具体实施方式

下面结合附图对本发明作更进一步的说明。

如图1所示，使用陀螺仪计算航向角，假设陀螺仪第i次测得的载体坐标系三个轴向上的角速度分别为w_xi、w_yi、w_zi，根据这三个分角速度值求得当前时刻的总角速度值为

$w_i=\sqrt{w_{\mathrm{xi}}^2+w_{\mathrm{yi}}^2+w_{\mathrm{zi}}^2}$

设立阈值W＝m·g，其中，m＝0.001～0.002，g为重力加速度，判断w_i与阈值W的大小情况，如果w_i＞W，则利用对Z轴向上的角速度值进行时间上的积分，为

其中，为前一时刻计算得到的航向角，为该时刻的航向角，T为陀螺仪的采样间隔。如果w_i≤W，则不进行积分。

如图2所示，使用磁力计计算航向角，首先根据h_x、h_y与0的大小关系以及h_x与h_y之间的大小关系将地理坐标系水平面按照如图3所示划分为8个象限。然后在每个象限中等间隔选取n个角度值将磁力计水平摆放朝向这n个角度，测量得到当前h_rx、h_ry的值，根据公式计算n个角度真实值计算这n个测量值与真实值之间的差值为根据公式计算第d个象限的修正因子Q_d，其中，q_k为第k个角度的角度值与真实值之间的差值。

设加速度计第i次测得的载体坐标系三个轴向上的加速度值分别为a_xi,a_yi,a_zi,则可以得到该载体的俯仰角θ和翻滚角γ分别为

$\mathrm{\θ}={\tan }^{-1}\frac{a_{\mathrm{xi}}}{\sqrt{a_{\mathrm{yi}}^2+a_{\mathrm{zi}}^2}}$

$\mathrm{\γ}={\tan }^{-1}\frac{a_{\mathrm{yi}}}{\sqrt{a_{\mathrm{xi}}^2+a_{\mathrm{zi}}^2}}$

设磁力计第i次测得的载体坐标系三个轴向上的磁场强度值分别为h_xi,h_yi,h_zi，利用下面的公式将其投影到地理坐标系的水平面上，得到该水平面上x轴与y轴方向上的磁场强度值h_xm和h_ym分别为

h_xm＝h_xi·cosγ+h_yi·sinγ

h_ym＝h_xi·sinθ·sinγ+h_yi·cosθ-h_zi·sinθ·cosγ

上述两个公式与对坐标系的定义以及坐标系之间的转换顺序有关，可根据实际情况采用不同的计算公式。

根据计算得到的h_xm与h_ym的值将其对应到图2的某一象限，使用该象限的修正因子对计算得到的航向角进行修正，计算公式为

以下四种情况需单独考虑：

h_xm＞0，h_ym＝0时，

h_xm＜0，h_ym＝0时，

h_xm＝0，h_ym＜0时，

h_xm＝0，h_ym＞0时，

如图4所示，将上述两种方法获得的航向角进行融合，由于在动态运动时，陀螺仪测量得到的航向角比较准确，而在静止或者缓慢运动时，磁力计能够计算得到比较准确的航向角。因而结合行人运动时的角速度变化情况，采用固定长度的滑动窗口，假设窗口长度为N，W_Z＝[w_z1,w_z2,...,w_zN]，每次测量得到新的载体坐标系z轴方向的角速度值w_zi时，将最新测量得到的值加入到窗口的最末端，并将窗口内最先得到的值移除，即W_Z＝[w_z2,w_z3,...,w_zi]，求得此刻窗口内所有角速度值的方差为w_var，利用该方差值设置加权系数 $k_{\mathrm{gyr}}=\frac{{\mathrm{cw}}_{\mathrm{var}}}{{\mathrm{cw}}_{\mathrm{var}}+1},k_{\mathrm{com}}=\frac{1}{{\mathrm{cw}}_{\mathrm{var}}+1},$ 其中，c＝5。

根据公式结合上述加权系数对使用陀螺仪和磁力计分别计算得到的航向角进行加权融合，得到最终的航向角

本方法使用的惯性传感器包括三轴加速度计、三轴陀螺仪以及三轴磁力计，其中加速度计测量的是载体坐标系三个轴向上的加速度值，陀螺仪测量的是载体坐标系三个轴向上的角速度值，磁力计测量的是载体坐标系三个轴向上的磁场强度值。

将本发明的航向角计算方法应用到航迹推算算法中对行人进行定位，在该算法中控制步伐检测方法以及步长模型不变，只改变航向角的计算方法，对使用本发明提供的方法与仅使用陀螺仪或磁力计计算的航向角进行比较，三者的定位效果的不同。

如图5所示，对行人的真实轨迹行走进行定位，采用正向过零检测算法进行步伐检测以及统一的非线性步长模型，分别使用陀螺仪、磁力计以及本发明提供的航向角计算方法计算航向角，这三种方法的定位结果比较，可以看出采用本发明提供的航向角计算方法，定位精度得到提高。

以上所述仅为本发明的较佳实施方式，本发明的保护范围并不以上述实施方式为限，但凡本领域普通技术人员根据本发明所揭示内容所作的等效修饰或变化，皆应纳入权利要求书中记载的保护范围内。

Claims (4)

1.一种基于惯性传感器的航向角获取方法，其特征在于：包括以下步骤：

步骤10：采用陀螺仪测量载体坐标系三轴向上的角速度；根据测得的角速度获取此刻的载体的航向角；

步骤20：采用磁力计测量载体所在的磁场强度值，根据测得的磁场强度值获取此刻的载体的航向角；

步骤30：将步骤10和步骤20获得的航向角进行加权融合获得最后的航向角。

2.根据权利要1所述的基于惯性传感器的航向角获取方法，其特征在于：所述步骤10中获得载体的航向角的方法为：

步骤101：设定总角速度阀值W＝m·g，其中，m＝0.001～0.002，g为重力加速度；

步骤102：将陀螺仪测得的载体坐标系三个轴向上的角速度值求取总角速度值，将获得的总角速度值与步骤101中设定的总角速度阀值进行比较，如果总角速度值大于总角速度阀值，对载体坐标系z轴方向上的角速度值进行时间上的积分得到此时载体的航向角。

3.根据权利要1所述的基于惯性传感器的航向角获取方法，其特征在于：所述步骤20中获得载体的航向角的方法为：

步骤201：将地理坐标系水平面划分为8个象限；

步骤202：在每个象限中等间隔选取n个角度，n个角度值为将磁力计水平摆放朝向这n个角度，分别测量在不同角度时，水平面上x轴与y轴方向上的磁场强度值h_rx和h_ry；

步骤203：根据公式计算n个角度真实值计算n个角度值与真实值之间的差值为

步骤204：根据公式计算第d个象限的修正因子Q_d，其中，q_k为第k个角度的角度值与真实值之间的差值；

步骤205：在每个象限中分别采用每个象限对应的修正因子对航向角进行修正。

4.根据权利要1所述的基于惯性传感器的航向角获取方法，其特征在于：所述步骤30中通过加权融合获得最后的航向角的方法为：

步骤301：设定长度为的滑动窗口，其中，T为陀螺仪的数据采样时间间隔，以秒为单位；

步骤302：将陀螺仪每次测量得到新的载体坐标系z轴方向的角速度值w_zi时，将最新测量得到的值加入到窗口的最末端，并将窗口内最先得到的值移除；

步骤303：计算求得此时窗口内所有角速度值的方差为w_var，利用所得方差值设置加权系数c＝5，k_gyr为陀螺仪测量得到的航向角的加权系数，k_com为磁力计测量得到的航向角的加权系数；

步骤304：根据公式获得最终的航向角，其中，为最终的航向角，为陀螺仪测量得到的航向角，为磁力计测量得到的航向角。