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

权利要求书

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所述的一种基于室内定位装置的室内定位位置校正方法，其特征在于，权重归一化的公式为：

ω‾ki=ωkiΣi=1Sωki,]]>

S是权重不为0的粒子个数，为归一化后的第i个粒子的权重。

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

Xk=Σi=1SXkiω‾ki,]]>

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

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