[发明专利]一种基于室内定位装置的室内定位位置校正方法

说明书

技术领域

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

背景技术

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

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

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个粒子的东向坐标和北向坐标。

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