基于贝叶斯估计和地图辅助校准的步行者室内定位方法与流程

本发明涉及室内定位与导航的技术领域，特别是涉及一种基于贝叶斯估计和地图辅助校准的步行者室内定位方法。

背景技术：

基于惯性传感器的行人室内定位技术是一种自完备的无需预先安装配套设备的定位手段。正是由于这种特性，基于惯性导航系统的步行者室内定位技术越来越多得到了关注，并且在某些特定的场景下有着非常重要的应用前景，如应急救援、应急医疗、商场位置确定等场合。但是由于受到误差累积的困扰，这种技术在定位精度上一直无法满足人们的应用需求。

经过十多年的发展，人们提出了多种辅助定位的方法来减小或者试图消除误差的无限累积。无线定位辅助惯性定位是各种机构研究的比较热门的方向，但是这种辅助方法仍然无法摆脱需要预先布置的缺陷。

基于粒子滤波并结合室内地图信息辅助惯性定位的技术是另一种辅助定位的方法。该方法结合地图信息利用粒子滤波算法纠正无效位置点，以达到提高定位精度的目的。这种算法保留了基于惯性传感器定位的优势，也无需在建筑物内安装辅助定位的设施，并且遏制了由于传感器误差而造成的位置误差的无休止的积累，能得到相对较高的定位精度。唯一需要的先验知识就是一张建筑物的楼层结构图。在实际中可以很方便的得到建筑物的安全疏散示意图，甚至在特殊情况下可以根据熟悉改建屋的人的口头描述得到这样一张示意图；而且当前室内地图发展火热，有很多著名的公司都在着力构建丰富的地图信息。

但是上述算法仍然存在一定的应用难题和缺陷。首先，地图的表示方法以及存储方式对算法的实现有很大的影响。其次，对于像门这样的连通区域，粒子滤波的输出结果可能出现错误。

技术实现要素：

鉴于以上所述现有技术的缺点，本发明的目的在于提供一种基于贝叶斯估计和地图辅助校准的步行者室内定位方法，提出了一种地图结构化表示的方法，并在行人惯性导航算法的基础上结合粒子滤波，应用贝叶斯估计进行室内环境下步行者的定位追踪，进一步减小了位置和方向的误差累积，使基于惯性传感器的室内定位精度更高，实用性更强。

为实现上述目的及其他相关目的，本发明提供一种基于贝叶斯估计和地图辅助校准的步 行者室内定位方法，包括以下步骤：步骤S1、在被定位人员身上固定惯性测量单元；步骤S2、根据已知的建筑物安全疏散示意图，进行建筑物楼层的结构化表示；步骤S3、根据建筑物楼层的结构化表示，在地图上标定被定位人员的当前位置；步骤S4、以当前位置为坐标起点，根据惯性测量单元获取的数据实时推算被定位人员的位置；步骤S5、根据当前时刻计算的被定位人员的位置，计算当前位置所在的房间，并判断是否发生穿墙事件；如果是，转入步骤S6；如果否，转入步骤S4；步骤S6、通过贝叶斯估计判断该穿墙事件是由出门时的位置偏差引起的，还是由方向偏移导致的穿墙引起的；如果判断为由出门时的位置偏差引起的，则将位置修正到最近的门的位置，否则经过粒子滤波对位置进行修正。

根据上述的基于贝叶斯估计和地图辅助校准的步行者室内定位方法，其中：所述步骤S1中，所述惯性测量单元固定在所述被定位人员的脚上。

根据权利要求1所述的基于贝叶斯估计和地图辅助校准的步行者室内定位方法，其特征在于：所述步骤S2包括以下步骤：

21)对整个楼层所有房间按顺序进行编号，然后将楼层表示成房间的集合：

楼层＝{房间1，房间2，…，房间n}，其中n表示房间的总数；

22)将每个房间表示为多边形和连通关系的集合，其中，多边形用顶点的集合来表示，连通关系表示每个房间的各面墙是否与其他房间连通。

根据上述的基于贝叶斯估计和地图辅助校准的步行者室内定位方法，其中：所述惯性测量单元包括三轴加速度计、三轴陀螺仪和三轴磁力计；所述三轴加速度计用于检测物体的加速度，所述三轴陀螺仪用于检测物体在三维空间中的角速度；所述三轴磁力计用于测量三维空间的磁场强度。

进一步地，根据上述的基于贝叶斯估计和地图辅助校准的步行者室内定位方法，其中：所述步骤S4包括以下步骤：

41)通过三轴加速度计、三轴陀螺仪和三轴磁力计获取被定位人员的运动数据并输入到惯性导航系统；

42)惯性导航系统在静止时刻根据重力场估计惯性测量单元的倾斜角，并通过旋转矩阵将传感器坐标系下的加速度和磁场强度映射到惯性导航坐标系下；

43)对导航坐标系下的加速度进行积分得到速度v，对速度v再次积分得到未经过任何滤波的位置p，并通过三轴磁力计和三轴陀螺仪获得的数据的互补滤波得到行走方向角yaw：

44)对位置p进行零速更新，对速度v、位置p和行走方向yaw的误差进行估计；

45)分别去除速度v、位置p和行走方向yaw的误差。

当坐标系与墙面平行时，若x₁<x<x₂&&y₁<y<y₂，则表示当前位置在该房间中，没有穿墙；否则表示当前位置不在该房间中，发生穿墙，其中(x，y)为当前位置的坐标，[x₁，x₂]和[y₁，y₂]分别为该房间在x向和y向上的取值区间；

当坐标系不与墙面平行时，若则表示当前位置在该房间中，没有穿墙；否则表示当前位置不在该房间中，发生穿墙，其中(x，y)为当前位置的坐标，和表示该房间在y向上的边界线，和表示该房间在x向上的边界线。

根据上述的基于贝叶斯估计和地图辅助校准的步行者室内定位方法，其中：所述步骤S6中，贝叶斯估计包括以下步骤：

a)计算出门时的上一步的位置离最近的门的距离d，以及出门时行走方向与墙体的夹角

b)设定出门时落脚位置距门的距离服从均值为μ_dp＝0，方差为σ_dp＝0.2的正态分布，即d|pass～N(μ_dp,σ_dp)；出门时行走方向与墙体夹角服从均值为方差为的正态分布，即非出门时落脚位置距门的距离和非出门时行走方向与墙体夹角的概率分布分别为：d|obastraction～N(μ_do,σ_do)，其中μ_do,σ_do,分别表示非出门时落脚位置距门的距离和非出门时行走方向与墙体夹角的均值和方差；

c)计算出门的概率和非过门的概率并进行归一化；

d)如果则判断为由出门时的位置偏差引起的，否则判断为由方向偏移导致的穿墙引起的，其中和分别是出门的概率和非过门的概率归一化后的值。

进一步地，根据上述的基于贝叶斯估计和地图辅助校准的步行者室内定位方法，其中：出门的概率和非过门的概率分别为

归一化后的出门的概率和非过门的概率为

根据上述的基于贝叶斯估计和地图辅助校准的步行者室内定位方法，其中：所述步骤S6中，所述粒子滤波包括以下步骤：

a)检测位置的零速度状态，读取上一步零速度时刻的位置和行走方向信息，并利用当前时刻的位置和行走方向与上一步的位置和方向计算步长l和方向变化δφ；

b)通过采样得到l和δφ的随机分布，带入状态方程得到大量预测粒子；

c)根据l和δφ的概率分布计算各个粒子的权重，并对其进行归一化得到归一化权重ω_i；

d)判断各个粒子是否发生穿墙事件，并将穿墙的粒子的归一化权重修改为0；

e)对有效粒子进行加权平均得到滤波后的位置。

如上所述，本发明的基于贝叶斯估计和地图辅助校准的步行者室内定位方法，具有以下有益效果：

(1)提供了一种地图的结构化表示方式，以辅助粒子滤波判断粒子的有效性；

(2)应用贝叶斯估计实现了室内环境下步行者的定位追踪；

(3)具有自完备的特性，且精度更高；

(4)在应急救援以及日常生活中都有很大的应用价值。

附图说明

图1显示为本发明的基于贝叶斯估计和地图辅助校准的步行者室内定位方法的流程图；

图2显示为本发明的IMU的安装示意图；

图3显示为本发明的建筑物安全疏散示意图；

图4显示为本发明中的建筑物楼层的结构化表示的示意图；

图5显示为本发明中单个房间的结构示意图；

图6(a)显示为本发明中坐标系与房间平行的示意图；

图6(b)显示为本发明中坐标系不与房间平行的示意图；

图7显示为本发明的贝叶斯估计的效果示意图；

图8显示为本发明的粒子滤波的效果示意图。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。

需要说明的是，本实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。

本发明的基于贝叶斯估计和地图辅助校准的步行者室内定位方法在粒子滤波的基础上结合贝叶斯分类器，利用地图信息对行人室内定位误差进行纠正，从而达到更高的定位精度。具体地，设计一种地图的结构化表示方法，即将楼层表示成房间的集合，将房间表示成多边形和连通关系的集合，并将多边形表示成顶点的集合；通过这种表示方法可以很容易计算当前位置与房间的关系；同时，在此基础上利用粒子滤波消除由定位误差积累引起的不合理定位点，并利用贝叶斯网络检测出门点的位置；如果检测到穿墙事件发生，则对定位位置进行二次纠正，从而使基于惯性传感器的室内定位精度得到进一步提高。

参照图1，本发明的一种基于贝叶斯估计和地图辅助校准的室内定位算法包括以下步骤：

步骤S1、在被定位人员身上固定惯性测量单元(Inertial Measurement Unit，IMU)。

其中，IMU是集成了三轴加速度计、三轴陀螺仪和三轴磁力计的设备。通常，三轴加速度计用于检测物体在载体坐标系统独立三轴的加速度信号，而三轴陀螺仪检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度，并以此解算出物体的姿态。三轴磁力计用于测量三维空间的磁场强度。

同时，IMU上集成有蓝牙或者WiFi传输模块。通过蓝牙或者WiFi来获取传感器的数据。

优选地，如图2所示，可以将IMU固定在被定位人员的脚上。

步骤S2、根据已知的建筑物安全疏散示意图，进行建筑物楼层的结构化表示。其中，建筑物安全疏散示意图如图3所示。建筑物楼层的结构化表示如图4所示。

参照图4，步骤S2包括以下步骤：

21)对整个楼层所有房间按顺序进行编号，然后将楼层表示成房间的集合：

楼层＝{房间1，房间2，…，房间n}，其中n表示房间的总数。

22)将每个房间表示为多边形和连通关系的集合，即房间n＝{V,C}。

其中，多边形用顶点的集合来表示，即V＝{p₁,p₂,...,p_m}，m表示顶点的个数，p_i为多边形的顶点，表示为p_i＝{x_i,y_i}。

连通关系表示每个房间的各面墙是否与其他房间连通，即是否包含有门。具体地，C表示该房间与其他房间的连通关系，即C＝{c₁,c₂,...,c_k}。其中，k为表示该房间的多边形的边数。因此，可知k＝m。c_k表示该房间的第k堵墙壁与其他房间的连通关系，即该面墙是否有门，可表示为：其中，(x_door,y_door)表示连通情况下门的中点的位置坐标。

步骤S3、根据建筑物楼层的结构化表示，在地图上标定被定位人员的当前位置。

具体地，将步骤S2中完成的结构化表示的建筑物楼层的信息导入计算机，在地图上标定被定位人员在建筑物楼层的地图上的当前位置。

需要说明的是，步骤S1和步骤S2-S3的执行顺序不是固定的，可以根据实际情况，先后执行或同时执行。

步骤S4、以当前位置为坐标起点，根据惯性测量单元获取的数据实时推算被定位人员的位置。

其中，步骤S4具体包括以下步骤：

41)通过IMU中的三轴加速度计、三轴陀螺仪和三轴磁力计获取被定位人员的运动数据并输入到惯性导航系统(INS)。

其中，通过IMU上的蓝牙或WiFi将三轴加速度计、三轴陀螺仪和三轴磁力计获取被定位人员的运动数据传输到智能终端或计算机上的惯性导航系统。

42)惯性导航系统在静止时刻根据重力场估计IMU的倾斜角，并通过旋转矩阵C将传感器坐标系下的加速度a^sensor＝{a_x^sensor,a_y^sensor,a_z^sensor}和磁场强度m^sensor＝{m_x^sensor,m_y^sensor,m_z^sensor}映射到惯性导航坐标系下，映射后分别为：a^nav＝{a_x^nav,a_y^nav,a_z^nav}，m^nav＝{m_x^nav,m_y^nav,m_z^nav}，映射过程为其中C_k为方向转移矩阵。

43)对导航坐标系下的加速度进行积分得到速度v，对速度v再次积分得到未经过任何 滤波的位置p，并通过IMU的三轴磁力计和三轴陀螺仪的角速度和磁场强度互补滤波得到行走方向角yaw。

具体地，

$v_k=v_{k-1}+(a_k^{\mathrm{nav}}+a_{k-1}^{\mathrm{nav}}-2{(00g)}^T)\mathrm{\&Delta;t}/2$

p_k＝p_k-1+(v_k+v_k-1)Δt/2

其中，v_k表示k时刻的速度，p_k表示k时刻的位置。(0 0 g)^T表示水平放置设备时的重力加速度。(0,0)表示是水平方向，g是垂直方向的重力加速度，通常g＝9.8m/s²。

44)对位置p进行零速更新，对速度、位置和行走方向误差进行估计。

具体地，输出的位置p的累积误差会在短时间内导致位置估算产生严重错误，所以先通过陀螺仪阈值法进行零速度检测，并在零速度期间利用卡尔曼滤波器对速度、位置和行走方向误差进行估计。该过程称为零速更新(ZUPT)。

45)分别去除速度、位置和行走方向误差，以达到减小误差累积的目的。

步骤S5、根据当前时刻惯性导航系统输出的被定位人员的位置p＝{x,y}，计算当前位置所在的房间，并判断是否发生穿墙事件；如果是，转入步骤S6；如果否，转入步骤S4。

如图5所示，房间＝{V，C}，V＝{A1,A2,A3,A4}，C＝{c1,c2,c3,c4}，c1＝D，c2＝{0,0}，c3＝{0,0}，c4＝{0,0}，p表示当前位置。

具体地，若坐标系与墙面平行，如图6(a)所示，则：

其中(x，y)为当前位置的坐标，[x₁，x₂]和[y₁，y₂]分别为该房间在x向和y向上的取值区间。

若坐标系不与墙面平行，如图6(b)所示，则：

其中分别表示A1A2,A3A4,A2A3,A1A4所在直线的方程。

步骤S6、通过贝叶斯估计判断该穿墙事件是由出门时的位置偏差引起的，还是由方向偏 移导致的穿墙引起的；如果判断为由出门时的位置偏差引起的，则将位置修正到最近的门的位置，否则经过粒子滤波对位置进行修正。

经过贝叶斯估计判断为由于出门点位置偏差引起的时，对位置进行修正的结果示意图如图7所示。其中，x表示修正前的位置，√表示修正后的位置。

具体地，贝叶斯估计的过程如下：

a)计算出门时的上一步的位置离最近的门的距离d，以及出门时行走方向与墙体的夹角

b)设定出门时落脚位置距门的距离服从均值为μ_dp＝0，方差为σ_dp＝0.2的正态分布，即d|pass～N(μ_dp,σ_dp)；出门时行走方向与墙体夹角服从均值为方差为的正态分布，即非出门时落脚位置距门的距离和非出门时行走方向与墙体夹角的概率分布分别为：d|obastraction～N(μ_do,σ_do)，其中μ_do,σ_do,分别表示非出门时落脚位置距门的距离和非出门时行走方向与墙体夹角的均值和方差。

c)计算出门的概率和非过门的概率并进行归一化。

由朴素贝叶斯公式可得：

上述两个式子都包含因此在计算的过程中可以省略。

归一化后可得：

d)如果则判断为由出门时的位置偏差引起的，否则判断为由方向偏移导致的穿墙引起的，其中和分别是和归一化后的值。

粒子滤波具体包括以下步骤：

a)检测位置的零速度状态，读取出门时上一步零速度时刻的位置和方向信息，并利用当前时刻的位置和行走方向与前一步的位置和行走方向计算步长l和方向变化δφ。

b)通过采样得到l和δφ的随机分布，带入状态方程得到大量预测粒子。

c)根据l和δφ的概率分布计算各个粒子的权重，并对其进行归一化得到归一化权重ω_i。

d)判断各个粒子是否发生穿墙事件，并将穿墙的粒子的归一化权重修改为0。即：

具体地，通过计算当前所在位置点所在的房间以及各个粒子所在的房间，如果在不同的房间，则表示粒子发生穿墙事件，并将该粒子的权重置为零，否则判断为没有穿墙。

e)对有效粒子进行加权平均得到滤波后的位置。粒子滤波的效果图如图8所示。其中，x表示滤波前的位置，√表示滤波后的位置。

综上所述，本发明的基于贝叶斯估计和地图辅助校准的步行者室内定位方法提供了一种地图的结构化表示方式，以辅助粒子滤波判断粒子的有效性；应用贝叶斯估计实现了室内环境下步行者的定位追踪；具有自完备的特性，且精度更高；在应急救援以及日常生活中都有很大的应用价值。所以，本发明有效克服了现有技术中的种种缺点而具高度产业利用价值。

上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。