CN103791906A - 一种基于室内定位装置的室内定位位置校正方法 - Google Patents

Abstract

本发明公开了一种基于室内定位装置的室内定位位置校正方法，包括以下步骤：粒子初始化，设定定位对象初始位置，在定位对象初始位置按照正态分布随机生成N个粒子，并载入地图信息；读入定位装置得到的定位对象第k步的步长SL_k和航向根据状态方程对粒子预估计，得到第i个粒子的第k步的状态变量k为大于等于1的自然数，k初始值为1；i＝1,2,…,N，表示第i个粒子；粒子更新：分别判定每个粒子是否穿过障碍物，如果是，则将粒子权重设为0，否则粒子权重不变；进行权重归一化，并计算定位对象第k步的航位估计值X_k；判断权重不为0的粒子数量是否大于N/2，如果是，则将k加1后赋值给k，否则进行粒子重采样，并将k加1后赋值给k。

Description

一种基于室内定位装置的室内定位位置校正方法

技术领域

本发明涉及一种导航技术领域的累积误差消除方法，具体是一种基于粒子滤波的室内定位位置校正方法。

背景技术

随着社会和经济的飞速发展，人们对定位与导航的需求日益增加，而这种需求在消防领域内也正日益凸显。在当今重特大恶性灾害火灾事故和化学泄漏事故日益增多的形势下，面对各种类型的严峻灾害，如何在有效提升灾害救援的进度和效率的同时，又能最大限度地保障第一线消防员个人安危，正成为一个亟待解决的现实问题。显然，平时系统而扎实的消防演练和培训是解决这个问题的根本之道，但随着技术的进步，尤其是定位导航技术的进步，将为这个问题的解决提供更加科学的解决方案。

消防行业需要在现场紧急情况下保护消防队员的安全，调度中心知道消防队员的位置信息后，可以优化调度。该行业目前的状况是消防队员靠对讲机和调度中心联系，汇报位置很繁琐，并且得不到实时位置信息。重要的是，如果消防队员出了问题，调度中心无法知道其位置，也无法调度其他消防队员对其救援。反恐特警有着类似的需求，当特警进入建筑物内后，调度中心知道其位置就可以有效的调度。

PDR（行人导航推算，Pedestrian Dead Reckoning）系统采用MEMS（微机电系统,Micro-Electro-Mechanical System）传感器，以计步检测为基础，从已知的起始位置进行航位推算的相对导航技术。PDR系统不需要预装基础设施，可以在没有GPS信号的环境中满足定位需求。

地图匹配算法被广泛用于车载导航，行驶在道路上的汽车受到道路的约束使地图匹配算法行之有效。

现有技术的缺点：1）PDR系统的误差会随时间而累积。2）室内地图约束没有一成型方法。

现有的地图约束修正方法：对于“穿越墙壁”的轨迹，简单的做一些几何修正，并不能达到位置校正的理想效果。

发明内容

发明目的：本发明所要解决的技术问题是针对现有技术的不足，提供一种基于室内定位装置的室内定位位置校正方法。

为了解决上述技术问题，本发明公开了一种基于室内定位装置的室内定位位置校正方法，包括以下步骤：

步骤（1），粒子初始化，设定定位对象初始位置，在定位对象初始位置按照正态分布随机生成N个粒子，并载入地图信息；

步骤（2），读入定位装置得到的定位对象第k步的步长SL_k和航向

根据状态方程对粒子预估计，得到第i个粒子的第k步的状态变量

k为大于等于1的自然数，k初始值为1；i＝1,2,…,N，表示第i个粒子；

步骤（3），粒子更新：分别判定每个粒子是否穿过障碍物，如果是，则将粒子权重设为0，否则粒子权重不变，定位对象第k步时第i个粒子的权重

计算公式为：

步骤（4），进行权重归一化，并计算定位对象第k步的航位估计值X_k；

步骤（5），判断权重不为0的粒子数量是否大于N/2，如果是，则将k加1后赋值给k，否则进行粒子重采样，并将k加1后赋值给k，返回步骤（2）。

本发明粒子个数N＝500。

本发明所述地图信息为建筑三维地图信息，包括标识了障碍物位置、可以通行的区域、每一个房间的ID、房间的区域、房间门的坐标。

本发明步骤（2）中第i个粒子的状态方程的计算公式为：

SL_k、

分别为定位对象第k步的步长和航向，为定位对象第k-1步到第k步的航向变化，

为行走第k步前第i个粒子的东向坐标和北向坐标，为完成第k步后第i个粒子的东向坐标和北向坐标。

本发明权重归一化的公式为：

${\stackrel{\‾}{\mathrm{\ω}}}_k^i=\frac{{\mathrm{\ω}}_k^i}{\underset{i=1}{\overset{S}{\mathrm{\Σ}}}{\mathrm{\ω}}_k^i},$

S是权重不为0的粒子个数，

为归一化后的第i个粒子的权重。

本发明步骤（4）中，定位对象第k步的航位估计值X_k的计算公式为：

$X_k=\underset{i=1}{\overset{S}{\mathrm{\Σ}}}{X}_k^i{\stackrel{\‾}{\mathrm{\ω}}}_k^i,$

S是权重不为0的粒子个数。

本发明步骤（5）中，所述重采样为在当前定位对象的位置周围按照正态分布随机生成N个粒子，粒子个数N＝500。

有益效果：本发明基于室内定位装置的室内定位位置校正方法，消除室内定位的累积误差，可以达到以下有益效果：

（1）纠错能力强。传感器在室内易受到多种干扰，以致提供错误的位置信息，通过该发明方法可以有效修正室内行人的错误位置信息；

（2）实时性好。定位装置以计步为基础，定位对象行走一步，即可对这一步做出修正。

附图说明

下面结合附图和具体实施方式对本发明做更进一步的具体说明，本发明的上述和/或其他方面的优点将会变得更加清楚。

图1粒子权重更新示意图。

图2为本发明方法流程图。

图3为定位装置结构示意图。

图4为实施例效果示意图。

具体实施方式

如图2所示，本发明公开了一种基于室内定位装置的室内定位位置校正方法，包括以下步骤：

步骤（1），粒子初始化，设定定位对象初始位置，在定位对象初始位置按照正态分布随机生成N个粒子，并载入地图信息；

步骤（2），读入定位装置得到的定位对象第k步的步长SL_k和航向

根据状态方程对粒子预估计，得到第i个粒子的第k步的状态变量k为大于等于1的自然数，k初始值为1；i＝1,2,…,N，表示第i个粒子；

步骤（3），粒子更新：分别判定每个粒子是否穿过障碍物，如果是，则将粒子权重设为0，否则粒子权重不变，定位对象第k步时第i个粒子的权重

计算公式为：

步骤（4），进行权重归一化，并计算定位对象第k步的航位估计值X_k；

步骤（5），判断权重不为0的粒子数量是否大于N/2，如果是，则将k加1后赋值给k，否则进行粒子重采样，并将k加1后赋值给k，返回步骤（2）。

本发明粒子个数N＝500。

本发明所述地图信息为建筑三维地图信息，包括标识了障碍物位置、可以通行的区域、每一个房间的ID、房间的区域、房间门的坐标。

本发明步骤（2）中第i个粒子的状态方程的计算公式为：

SL_k、

分别为定位对象第k步的步长和航向，

为定位对象第k-1步到第k步的航向变化，

为行走第k步前第i个粒子的东向坐标和北向坐标，

为完成第k步后第i个粒子的东向坐标和北向坐标。

本发明权重归一化的公式为：

${\stackrel{\‾}{\mathrm{\ω}}}_k^i=\frac{{\mathrm{\ω}}_k^i}{\underset{i=1}{\overset{S}{\mathrm{\Σ}}}{\mathrm{\ω}}_k^i},$

S是权重不为0的粒子个数，

为归一化后的第i个粒子的权重。

本发明步骤（4）中，定位对象第k步的航位估计值X_k的计算公式为：

$X_k=\underset{i=1}{\overset{S}{\mathrm{\Σ}}}{X}_k^i{\stackrel{\‾}{\mathrm{\ω}}}_k^i,$

S是权重不为0的粒子个数。

本发明步骤（5）中，所述重采样为在当前定位对象的位置周围按照正态分布随机生成N个粒子，粒子个数N＝500。

实施例

如图3所示，实现本实施例的定位装置包括：一个STM32芯片，STM32芯片分别连接一个三轴数字加速度计、一个三轴数字陀螺仪、一个三轴数字罗盘，以及一个数字气压传感器。STM32芯片为主要功能元件。定位装置能够设置定位对象（行人）的初始位置，以及行人当前步的位移和航向信息，进行地图匹配约束后，通过本发明方法进行校准位置输出。

本实施例是通过以下技术方案实现的，本发明包括如下具体步骤：

步骤一：本实施例室内地图为某建筑六楼的地图。地图信息包括房间墙壁的轮廓存储为多边形，一个楼层中每个多边形代表一个房间。房间作为一个类，包含的信息有多边形各顶点的坐标，房间的ID、房间门的坐标，与该房间相邻的房间ID。另外，还存储所有已知物品的摆放信息。

步骤二：行人身上安装图3所示定位装置，供DR（Dead Reckoning，航位推算）算法使用。DR算法基于行人步态特征估计步长，联合航向信息推算行人的位置，实现定位需求。航位X_k推算公式为

式中，SL_k、

分别为第k步的步长和航向，

第k-1步到第k步的航向变化，E_k-1,N_k-1为行走第k步前的东向坐标和北向坐标，E_k,N_k为完成第k步后的东向坐标和北向坐标。

如图1和图2所示，步骤三：粒子滤波结合地图匹配，完成行人室内位置校正。

1）粒子初始化，设定出发位置，在初始位置随机生成N个粒子，粒子个数N＝500，并载入地图信息；

2）粒子预估计：读入一步的步长和航向，粒子的位置由状态方程公式(1)确定。

3）粒子更新：门是出入房间的唯一途径，通过算法规定，使没有通过门进入房间的粒子权重为0，有效得防止粒子穿过墙壁。

根据粒子的坐标判断粒子所在房间的ID。粒子权重的大小根据公式(2)更新，如图1所示，在图中直线上方的粒子，没有通过正常通路而是穿过障碍物，这些粒子的权重设为0，更新之后，剩余的粒子权重再做归一化处理。

式中，N是粒子的个数。

权重归一化为

${\mathrm{\ω}}_k^i=\frac{{\mathrm{\ω}}_k^i}{\underset{i=1}{\overset{S}{\mathrm{\Σ}}}{\mathrm{\ω}}_k^i}---\left(11\right)$

式中，S是粒子权重不为0的个数。

4）计算状态的估计值，判断S＞N/2是否成立，如果是，跳转到2）进入下一步的处理；如果否，则在估计值处重采样，重新生成N个粒子，跳转到2）进入下一步的处理；

5）定位结束。

得到实验结果如下图4所示：

图4中，标记“+”的线为PDR轨迹，标记“◇”的线为使用粒子滤波地图匹配算法后的轨迹。图中，标记“×”为轨迹的最后一步生成的粒子，标记“□”为轨迹起始点。

从图中可以看出，PDR轨迹在不做处理之前，发生了大约30°的偏转，使用粒子滤波地图匹配算法后，约束了PDR的发散，提高了PDR的定位精度。

本发明提供了一种基于室内定位装置的室内定位位置校正方法，具体实现该技术方案的方法和途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。

Claims (7)

1.一种基于室内定位装置的室内定位位置校正方法，其特征在于，包括以下步骤：

步骤（1），粒子初始化，设定定位对象初始位置，在定位对象初始位置按照正态分布随机生成N个粒子，并载入地图信息；

步骤（2），读入定位装置得到的定位对象第k步的步长SL_k和航向

根据状态方程对粒子预估计，得到第i个粒子的第k步的状态变量

k为大于等于1的自然数，k初始值为1；i＝1,2,…,N，表示第i个粒子；

步骤（3），粒子更新：分别判定每个粒子是否穿过障碍物，如果是，则将粒子权重设为0，否则粒子权重不变，定位对象第k步时第i个粒子的权重计算公式为：

步骤（4），进行权重归一化，并计算定位对象第k步的航位估计值X_k；

步骤（5），判断权重不为0的粒子数量是否大于N/2，如果是，则将k加1后赋值给k，否则进行粒子重采样，并将k加1后赋值给k，返回步骤（2）。

2.根据权利要求1所述的一种基于室内定位装置的室内定位位置校正方法，其特征在于，粒子个数N＝500。

3.根据权利要求1所述的一种基于室内定位装置的室内定位位置校正方法，其特征在于，所述地图信息为建筑三维地图信息，包括标识了障碍物位置、可以通行的区域、每一个房间的ID、房间的区域、房间门的坐标。

4.根据权利要求1所述的一种基于室内定位装置的室内定位位置校正方法，其特征在于，步骤（2）中第i个粒子的状态方程的计算公式为：

SL_k、

分别为定位对象第k步的步长和航向，

为定位对象第k-1步到第k步的航向变化，

为行走第k步前第i个粒子的东向坐标和北向坐标，

为完成第k步后第i个粒子的东向坐标和北向坐标。

5.根据权利要求4所述的一种基于室内定位装置的室内定位位置校正方法，其特征在于，权重归一化的公式为：

${\stackrel{\‾}{\mathrm{\ω}}}_k^i=\frac{{\mathrm{\ω}}_k^i}{\underset{i=1}{\overset{S}{\mathrm{\Σ}}}{\mathrm{\ω}}_k^i},$

S是权重不为0的粒子个数，

为归一化后的第i个粒子的权重。

6.根据权利要求5所述的一种基于室内定位装置的室内定位位置校正方法，其特征在于，步骤（4）中，定位对象第k步的航位估计值X_k的计算公式为：

$X_k=\underset{i=1}{\overset{S}{\mathrm{\Σ}}}{X}_k^i{\stackrel{\‾}{\mathrm{\ω}}}_k^i,$

S是权重不为0的粒子个数。

7.根据权利要求1所述的一种基于室内定位装置的室内定位位置校正方法，其特征在于，步骤（5）中，所述重采样为在当前定位对象的位置周围按照正态分布随机生成N个粒子，粒子个数N＝500。

Images