CN111024075A - 一种结合蓝牙信标和地图的行人导航误差修正滤波方法 - Google Patents

Abstract

本发明一种结合蓝牙信标和地图的行人导航误差修正滤波方法，步骤如下(1)采用陀螺仪、加速度计和磁强计，利用行人导航推算算法和卡尔曼滤波算法，计算行人航迹信息，包括：行人行走计步、步长、航向，和位置信息；(2)蓝牙设备设置在室内固定位置，能够向外辐射蓝牙信号，根据行人身上接收蓝牙信号的强度，得到蓝牙定位信息；(3)根据步骤(1)的行人航迹信息、步骤(2)的蓝牙定位信息及建筑地图信息，采用粒子滤波算法，得到更准确的行人位置信息，提高了室内行人导航定位精度。

Description

一种结合蓝牙信标和地图的行人导航误差修正滤波方法

技术领域

本发明涉及一种结合蓝牙信标和地图的行人导航误差修正滤波方法，属于室内行人导航定位领域。

背景技术

PDR方法，基于行人步态特征计算行走的步数和估计步长，联合航向信息推算行人的位置的一种方法。其主要原理是利用及速度计、陀螺仪和磁力计等传感器实现的一种定位算法，具有较高的精确性和鲁棒性，同时不需要对外辐射能量。但由于惯性传感器的固有属性，加速度计和陀螺仪在温度变化和时间变化的条件下都会出现较大的累积误差，因此，PDR在行人航位推算中，用户的位置估计会随着时间和距离的变化呈现难以估计的累积误差。

根据前述PDR系统在长时间导航过程中定位精度会严重下降，无法正确地进行导航及定位等功能，甚至还会提供错误的定位及导航信息给使用者，使使用者身陷迷惘之中。

发明内容

本发明解决的技术问题为：克服上述现有技术的不足，提供一种结合蓝牙信标和地图的行人导航误差修正滤波方法，提高了室内行人导航定位精度。

本发明解决的技术方案为：一种结合蓝牙信标和地图的行人导航误差修正滤波方法，步骤如下：

(1)采用陀螺仪、加速度计和磁强计，利用行人导航推算算法和卡尔曼滤波算法，计算行人航迹信息(包括：行人行走计步、步长、航向)和位置信息；

(2)蓝牙设备设置在室内固定位置，能够向外辐射蓝牙信号，根据行人身上接收蓝牙信号的强度，得到蓝牙定位信息；

(3)根据步骤(1)的行人航迹信息、步骤(2)的蓝牙定位信息及建筑地图信息，采用粒子滤波算法，得到更准确的行人位置信息，实现行人导航误差修正滤波。

优选的，步骤(1)在行人腿部绑上行人穿戴设备，包括：陀螺仪、加速度计和磁强计；采用陀螺仪、加速度计和磁强计，利用行人导航推算算法和卡尔曼滤波算法，计算行人航迹信息(包括：行人行走计步、步长、航向)和位置信息，具体如下：

(101)根据陀螺仪输出的角速度，加速度计输出的加速度，磁强计输出的磁强度，计算行人穿戴设备的磁航向角度、惯性航向角度和行人的位置信息；

(102)将行人穿戴设备的陀螺仪漂移和姿态四元数作为状态变量；将行人穿戴设备的加速度计、磁强计的测量误差作为观测量，根据给定的状态矩阵和观测矩阵，利用卡尔曼滤波算法，得到惯性航向角漂移这一状态变量的估计值，

(103)将(102)得到的陀螺仪漂移和姿态四元数的估计值，补偿到行人穿戴设备的惯性航向角度中，并根据(101)中的行人的位置信息和补偿后的惯性航向角度，得到较为准确的行人的位置信息；

优选的，行人航迹信息(包括：行人行走计步、步长、航向)

优选的，步骤(101)根据陀螺仪输出的角速度，加速度计输出的加速度，磁强计输出的磁强度，计算行人穿戴设备的磁航向角度、惯性航向角度和行人的位置信息，具体为：通过陀螺仪输出的角速度和加速度计输出的加速度，计算行人穿戴设备的惯性航向角度，并初步得到行人的位置信息；通过磁强计输出的磁强度和加速度计输出的加速度，计算得到行人穿戴设备的磁航向角度。

优选的，步骤(2)蓝牙设备设置在室内固定位置，能够向外辐射蓝牙信号，根据行人身上接收蓝牙信号的强度，得到蓝牙定位信息，具体如下：

(104)行人穿戴设备内置的接收器，接收蓝牙信号；根据接收的蓝牙信号的强度，通过路径损耗模型和加权最小二乘法，得到蓝牙定位信息。

优选的，(3)根据步骤(1)的行人航迹信息、步骤(2)的蓝牙定位信息及建筑地图信息，采用粒子滤波算法，得到更准确的行人位置信息，具体如下：

(105)以(104)得到的行人的蓝牙位置信息为中心，进行粒子采样，并利用(101)获得的航迹信息对每个粒子的状态量进行估计，得到每个粒子的位置信息；

(106)根据每个粒子位置信息与(104)得到的行人的蓝牙定位信息之间的距离，计算每个粒子的测量权值；根据建筑地图信息和(105)得到的每个粒子位置信息，计算每个粒子的地图权重；利用每个粒子的测量权值和每个粒子的地图权重，计算每个粒子的综合权重；

(107)利用(105)得到的每个粒子位置信息和(106)得到的每个粒子的综合权重，将所有粒子进行加权平均，得到行人的位置信息。

本发明与现有技术相比的优点在于：

(1)本发明提出了一种基于二阶级联卡尔曼滤波和粒子滤波的惯性导航、磁强计和地图信息的融合定位算法。该算法第一级利用EKF融合MEMS陀螺仪、MEMS加速度计和磁强计，其中磁强计的航向角作为EKF的观测量辅助PDR定位，从而得到较为准确的用户定位；第二级利用PF融合第一级得到的定位结果、蓝牙和地图，其中第一级得到的结果和地图信息用来计算PF的权重，最终得到更精确的用户位置。

(2)本发明利用RSSI定位信息为行人导航推算提供位置的绝对信息。利用行人导航推算算法能够获得行人的位置信息为当前步态相对于初始步态时的相对位置信息。本发明融合了RSSI定位信息，建筑地图信息，利用粒子滤波进行航迹推算，能够得到行人的绝对位置，

(3)本发明利用粒子滤波算法对PDR航迹推算进行校正。该算法不仅利用地图信息对行人行进过程中的位置进行校正，同时，对陀螺仪漂移导致的航向角度偏差进行在线估计。能够消除在是室内行人导航中地磁受到磁干扰导致的航向漂移的影响。

附图说明

图1是本发明中融合算法结构图；

图2是本发明中PDR算法框图；

图3是本发明中整体算法流程图；

图4是本发明利用惯性导航得到的行走轨迹；

图5是本发明利用惯性/蓝牙/建筑地图信息得到的行走轨迹。

具体实施方式

下面结合附图和具体实施例对本发明做进一步详细描述。

如图1所示，本发明一种结合蓝牙信标和地图的行人导航误差修正滤波方法，步骤如下(1)采用陀螺仪、加速度计和磁强计，利用行人导航推算算法和卡尔曼滤波算法，计算行人航迹信息，包括：行人行走计步、步长、航向，和位置信息；(2)蓝牙设备设置在室内固定位置，能够向外辐射蓝牙信号，根据行人身上接收蓝牙信号的强度，得到蓝牙定位信息；(3)根据步骤(1)的行人航迹信息、步骤(2)的蓝牙定位信息及建筑地图信息，采用粒子滤波算法，得到更准确的行人位置信息，提高了室内行人导航定位精度。

载体坐标系Ox_by_bz_b，原点与载体质心重合。Ox_b沿载体纵轴向前，Oy_b沿载体横轴向左，Oy_Z沿载体竖轴向下。

导航坐标系Ox_gy_gz_g，原点与载体质心重合。Ox_g指向地理东向，Oy_g指向地理北向，Oz_g指向当地地理垂线方向。

航向角度：定义载体绕线方向转动，载体的纵轴在水平面上的投影与地理北向之间的夹角。

磁航向角度：利用磁强计输出的磁强度计算得到的航向角度。

惯性航向角度：由陀螺仪输出的角速度和加速度计输出的加速度计算得到的航向角度。

行人穿戴设备：穿戴在行人身上的装置，内置陀螺仪、加速度计和磁强计，内置软件系统，软件系统可完成对陀螺仪、加速度计和磁强计的数据采集和处理，可完成对公式的计算。

本发明的一种结合蓝牙信标和地图的行人导航误差修正滤波方法，优选步骤如下：

步骤(1)在行人腿部绑上行人穿戴设备，包括：陀螺仪、加速度计和磁强计；采用陀螺仪、加速度计和磁强计，利用行人导航推算算法和卡尔曼滤波算法，计算行人航迹信息(包括：行人行走计步、步长、航向)和位置信息，如图2所示，优选方案如下：

(101)根据陀螺仪输出的角速度，加速度计输出的加速度，磁强计输出的磁强度，计算行人穿戴设备的磁航向角度、惯性航向角度和行人的位置信息；具体为：通过陀螺仪输出的角速度和加速度计输出的加速度，计算行人穿戴设备的惯性航向角度，并初步得到行人的位置信息；通过磁强计输出的磁强度和加速度计输出的加速度，计算得到行人穿戴设备的磁航向角度，优选方案如下：

ω_x，ω_y，ω_z是陀螺仪在载体坐标系的三轴输出角速度，k是指前一个采样点，k+1是指后一个采样点，t_k是前一个采样点的采样时间，t_k+1是后一个采样点的采样时间Δθ_x，Δθ_y，Δθ_z是[t_k，t_k+1]采样时间间隔内的三轴角速度增量。Δθ为[t_k，t_k+1]采样时间间隔内的角速度增量的矢量和。

Q是姿态四元数，是一个四阶矢量，可表示为Q＝[q₀ q₁ q₂ q₃]，Q(t_k)是前一采样点的姿态四元数，Q(t_k+1)是后一采样点的姿态四元数。

为惯性航向角度.

磁传感器输出的载体坐标系下的磁强度m^b，由下式可知，先转换为地理坐标下的磁强度m^g，在求得磁航向角度

其中，

分别为地理坐标系下三轴的磁强度。

式中，t_k，t_k-1分别为步态检测到的候选参考点的时间，α、β是行人步长模型参数。α、β对于每一个人是常量，但不同用户之间各不相同，需通过采集的数据进行统计分析，利用最小二乘拟合的方法估计。

(102)将行人穿戴设备的陀螺仪漂移和姿态四元数作为状态变量；将行人穿戴设备的加速度计、磁强计的测量误差作为观测量，根据给定的状态矩阵和观测矩阵，利用卡尔曼滤波算法，得到惯性航向角漂移这一状态变量的估计值，优选方案如下：

将陀螺仪漂移[Δx_gxΔx_gyΔx_gz]和姿态四元数Q作为卡尔曼滤波器的状态量X，将行人穿戴设备的加速度计、磁强计的测量误差作为卡尔曼滤波的观测量Z；

式中，y_a，y_m为载体系下加速度和磁强计的测量值，G为导航系中的重力场强度，M为导航系下的地磁场强度。

确定上述变量后，通过下述卡尔曼滤波四个方程在每个滤波周期内进行递推计算，就可以获得k时刻的状态估计值

即陀螺仪的漂移的估计值。

卡尔曼滤波器算法优选如下：

Step1：

Step2：

K_k＝P_k|k-1H_k[H_kP_k|k-1[H_k]^T+R_k]^-1

Step3：

Step4：

Step5：

P_k＝P_k|k-1-K_k(H_kP_k|k-1)

利用卡尔曼滤波算法对陀螺仪漂移和姿态四元数进行估计，得到[Δx_gx Δx_gy Δx_gz]和

(103)将(102)得到的陀螺仪漂移[Δx_gx Δx_gy Δx_gz]和姿态四元数

的估计值，补偿到行人穿戴设备的惯性航向角度中，并根据(101)中的行人的位置信息和补偿后的惯性航向角度，得到较为准确的行人的位置信息，优选方案如下：

在第j个步态周期，将(102)中得到的陀螺仪漂移[Δx_gx Δx_gy Δx_gz]和姿态四元数

利用下式，改进惯性航向角度，得到第j个步态周期的航向角度

和姿态四元数。

步骤(2)蓝牙设备设置在室内固定位置，能够向外辐射蓝牙信号，根据行人身上接收蓝牙信号的强度，得到蓝牙定位信息，具体如下：

(104)行人穿戴设备内置的接收器，接收蓝牙信号；根据接收的蓝牙信号的强度，通过路径损耗模型和加权最小二乘法，得到蓝牙定位信息，优选方案如下：

在室内架设信标节点(3个以上)，在第j个步态周期内，行人在经过信标节n1时，信标节点n1会收到行人的定位请求。此时，n1与邻近的两个节点n2、n3通讯，利用通讯的RSSI信息和布设时获知的相对距离，计算出当前步态周期的路径衰减指数β_j。

在当前步态周期，行人穿戴设备分别与邻近的n1、n2和n3节点通讯，利用RSSI值和路径衰减指数β_j，计算行人与节点n1、n2和n3的距离d1、d2和d3。并利用d1、d2和d3，根据三边定位原理，计算行人的蓝牙位置，可表示为(x_β，y_β)。

(3)根据步骤(1)的行人航迹信息、步骤(2)的蓝牙定位信息及建筑地图信息，采用粒子滤波算法，得到更准确的行人位置信息，流程图如图3所示，具体如下：

(105)以(104)得到的行人的蓝牙位置信息为中心，进行粒子采样，(101)获得的航迹信息对每个粒子的状态量进行估计，得到每个粒子的位置信息，优选方案如下：

首先，对粒子集进行初始化。

生成采样粒子

表示第j个步态周期第i个粒子的权重，

代表每个粒子具有的状态信息。其中，Δx_j Δy_j ΔV_xj ΔV_yj Δa_xj Δa_yj分别表示第j个步态周期第i个粒子的位置误差，速度误差和加速度误差，

分别表示第j个步态周期第i个粒子的航向角度误差和角速度误差。

利用步骤(104)第j个步态周期的蓝牙定位结果(x_β，y_β)和第j-1个步态周期步骤(107)计算得到的粒子滤波结果

作为粒子状态估计的量测值。具体的状态方程和量测方程可由下式给出，利用下述公式在每个步态周期对每个采样粒子的状态信息进行逐步更新。

式中，n_x、n_y、n_vx、n_vy、n_ax、n_ay、

n_ω、m_xβ、m_yβ为高斯白噪声。

(106)根据每个粒子位置信息与(101)得到的行人的PDR定位结果信息之间的距离，计算每个粒子的测量权值；根据建筑地图信息和(105)得到的每个粒子位置信息，计算每个粒子的地图权重；利用每个粒子的测量权值和每个粒子的地图权重，计算每个粒子的综合权重，优选方案如下：

利用(105)中估计的第j个步态周期第i个粒子的位置信息

和(104)中的得到的第j个步态周期的蓝牙定位信息(x_BT，y_BT)，计算得到第j个周期每个粒子i的测量权重

即

取(105)中估计的第j个步态周期的的位置信息

与第j-1个步态周期的位置信息

将这两个坐标构成的线段与建筑地图信息进行匹配计算。根据是否与邻近区域的建筑地图发生碰撞，判断第j个周期中每个粒子i的地图权重

即

根据测量权重和地图权重，计算第j个步态周期第i个粒子的综合权重Wⁱ。

(107)利用(105)得到的每个粒子位置信息和(106)得到的每个粒子的综合权重，将所有粒子进行加权平均，得到行人的位置信息，优选方案如下：

在每个步态周期j中，利用(105)中估计得到的每个粒子i的定位信息

和(106)计算得到的每个粒子i的综合权重Wⁱ，将所有粒子进行加权平均，得到第j个步态周期行人的位置信息

在本发明中算法开发和验证阶段，发明人进行了大量的室内行走测试。下图4和图5是其中的一组测试结果。该测试场景为大型商场，测试人员穿戴导航设备，并在商场多出设置蓝牙设备，模拟正常室内行走，采集传感器数据，利用PDR算法和PDR/蓝牙/地图算法对采集的数据进行对比验证。多次验证结果显示，仅利用PDR算法的导航结果漂移严重，且随时间发散，1小时导航精度仅为2％，利用本发明算法的算法很好的修正了PDR算法中陀螺随时间的漂移，1小时导航精度可达到0.5％，精度较仅用PDR算法提高了4倍。

Claims (6)

1.一种结合蓝牙信标和地图的行人导航误差修正滤波方法，其特征在于步骤如下：

(1)采用陀螺仪、加速度计和磁强计，利用行人导航推算算法和卡尔曼滤波算法，计算行人航迹信息和位置信息；

(2)蓝牙设备设置在室内固定位置，能够向外辐射蓝牙信号，根据行人身上接收蓝牙信号的强度，得到蓝牙定位信息；

(3)根据步骤(1)的行人航迹信息、步骤(2)的蓝牙定位信息及建筑地图信息，采用粒子滤波算法，得到更准确的行人位置信息。

2.根据权利要求1所述的一种结合蓝牙信标和地图的行人导航误差修正滤波方法，其特征在于：步骤(1)在行人腿部绑上行人穿戴设备，包括：陀螺仪、加速度计和磁强计；采用陀螺仪、加速度计和磁强计，利用行人导航推算算法和卡尔曼滤波算法，计算行人航迹信息和位置信息，具体如下：

(101)根据陀螺仪输出的角速度，加速度计输出的加速度，磁强计输出的磁强度，计算行人穿戴设备的磁航向角度、惯性航向角度和行人的位置信息；

(102)将行人穿戴设备的陀螺仪漂移和航向角偏差作为状态变量；将行人穿戴设备的磁航向角度和惯性航向角度的差作为观测量，根据给定的状态矩阵和观测矩阵，利用卡尔曼滤波算法，得到陀螺仪漂移和航向角偏差的估计值；

(103)将(102)得到的陀螺仪漂移和航向角偏差的估计值，补偿到行人穿戴设备的惯性航向角度中，并根据(101)中的行人的位置信息和补偿后的惯性航向角度，得到较为准确的行人的位置信息。

3.根据权利要求2所述的一种结合蓝牙信标和地图的行人导航误差修正滤波方法，其特征在于：行人航迹信息(包括：行人行走计步、步长、航向)。

4.根据权利要求2所述的一种结合蓝牙信标和地图的行人导航误差修正滤波方法，其特征在于：步骤(101)根据陀螺仪输出的角速度，加速度计输出的加速度，磁强计输出的磁强度，计算行人穿戴设备的磁航向角度、惯性航向角度和行人的位置信息，具体为：通过陀螺仪输出的角速度和加速度计输出的加速度，计算行人穿戴设备的惯性航向角度，并初步得到行人的位置信息；通过磁强计输出的磁强度和加速度计输出的加速度，计算得到行人穿戴设备的磁航向角度。

5.根据权利要求1所述的一种结合蓝牙信标和地图的行人导航误差修正滤波方法，其特征在于：步骤(2)蓝牙设备设置在室内固定位置，能够向外辐射蓝牙信号，根据行人身上接收蓝牙信号的强度，得到蓝牙定位信息，具体如下：

(104)行人穿戴设备内置的接收器，接收蓝牙信号；根据接收的蓝牙信号的强度，通过路径损耗模型和加权最小二乘法，得到蓝牙定位信息。

6.根据权利要求5所述的一种结合蓝牙信标和地图的行人导航误差修正滤波方法，其特征在于：(3)根据步骤(1)的行人航迹信息、步骤(2)的蓝牙定位信息及建筑地图信息，采用粒子滤波算法，得到更准确的行人位置信息，具体如下：

(105)以(104)得到的行人的蓝牙位置信息为中心，进行粒子采样，并利用(101)获得的航迹信息对每个粒子的状态量进行估计，得到每个粒子的位置信息；

(106)根据每个粒子位置信息与(104)得到的行人的蓝牙定位信息之间的距离，计算每个粒子的测量权值；根据建筑地图信息和(105)得到的每个粒子位置信息，计算每个粒子的地图权重；利用每个粒子的测量权值和每个粒子的地图权重，计算每个粒子的综合权重；

(107)利用(105)得到的每个粒子位置信息和(106)得到的每个粒子的综合权重，将所有粒子进行加权平均，得到行人的位置信息。

Images