基于地图信息和位置自适应修正的粒子滤波室内定位方法与流程

技术特征：

1.一种基于地图信息和位置自适应修正的粒子滤波室内定位方法，其特征在于：所述定位方法包括以下步骤：

步骤1，粒子初始化为指定粒子携带的状态信息为其中是初始时刻t₀第i个粒子的状态向量，分别是时刻t₀第i个粒子的横坐标、纵坐标和权重，初始权重设为1，N是粒子数量，是由目标初始位置加上偏移量得到，偏移量服从均值为0，方差为σ²的高斯分布；

步骤2，粒子传播，根据式(1)由k-1时刻粒子状态向量S_k-1计算出粒子在k时刻的状态向量S_k，其中是k时刻第i个粒子的坐标，是k-1时刻第i个粒子的坐标，L_k-1是k-1时刻目标的行进步长，L_k是k时刻目标的行进步长，由式(2)计算得到，其中α，β是步长模型参数，是在离线阶段根据已知步长和步频通过线性回归计算得到，freq_k是k时刻的目标步频，根据目标行进过程中采集到的加速度传感器数据通过波峰检测算法计算得到，是k时刻第i个粒子的航向，是k-1时刻第i个粒子的航向，由方向传感器采集得到；

$\left\{\begin{array}{c}{x}_k^i=x_{k-1}^i+L_{k-1}\·sin\left({\mathrm{\θ}}_{k-1}^i\right)\\ y_k^i=y_{k-1}^i+L_{k-1}\·cos\left({\mathrm{\θ}}_{k-1}^i\right)\end{array}\right.---\left(1\right)$

L_k＝α·freq_k+β (2)

步骤3，粒子更新，判断粒子在传播过程中是否与地图中的墙壁等障碍物相交，如果存在相交则该粒子灭亡，权重置为0；

步骤4，目标位置确定，根据式(3)计算得到目标在k时刻的最终位置，其中(X_k，Y_k)是k时刻由粒子滤波算法估算出的目标最终位置坐标，是k时刻第i个粒子的坐标，是k时刻第i个粒子的权重；

$\left\{\begin{array}{c}{X}_k=\underset{i=1}{\overset{N}{\Σ}}{x}_k^i\·w_k^i\\ Y_k=\underset{i=1}{\overset{N}{\Σ}}{y}_k^i\·w_k^i\end{array}\right.---\left(3\right)$

步骤5，粒子重采样和位置自适应修正，设步骤3中得到的存活粒子数为N_s，在存活的粒子中随机抽取N-N_s个粒子用于补偿损失的粒子；在粒子补偿时对补偿的粒子位置根据式(4)进行调整，其中是在k时刻随机抽取的存活粒子的位置坐标，是在k时刻补偿粒子的位置坐标，是k时刻的航向偏差修正参数，分别是的x轴分量和y轴分量，由式(5)计算得到，是k时刻重采样粒子i的位置偏移量，服从均值为0，方差为σ²的高斯分布，分别是的x轴分量和y轴分量；是k时刻存活粒子位置距离目标位置的平均偏移量，分别是的x轴分量和y轴分量，是k-1时刻重采样后粒子位置距离目标位置的平均偏移量，分别是的x轴分量和y轴分量；μ_k是k时刻的位置修正增益参数，由式(10)计算得到，其中表示

$\left\{\begin{array}{c}{x}_k^r=x_k^s+{\mathrm{\μ}}_k\·{\left({\mathrm{\θ}}_k^{bias}\right)}_x+{\left({\mathrm{\ϵ\θ}}_k^i\right)}_x\\ y_k^r=y_k^s+{\mathrm{\μ}}_k\·{\left({\mathrm{\θ}}_k^{bias}\right)}_y+{\left({\mathrm{\ϵ\θ}}_k^i\right)}_y\end{array}\right.---\left(4\right)$

${\mathrm{\θ}}_k^{bias}={\mathrm{\θ}}_k^{avg\_s}-{\mathrm{\θ}}_{k-1}^{avg\_r}---\left(5\right)$

${\left({\mathrm{\θ}}_k^{avg\_s}\right)}_x=\frac{1}{N_s}\·\underset{i=1}{\overset{N}{\Σ}}{x}_k^i-X_k---\left(6\right)$

${\left({\mathrm{\θ}}_k^{avg\_s}\right)}_y=\frac{1}{N_s}\·\underset{i=1}{\overset{N}{\Σ}}{y}_k^i-Y_k---\left(7\right)$

${\left({\mathrm{\θ}}_{k-1}^{avg\_r}\right)}_x=\frac{1}{N}\·\underset{i=1}{\overset{N}{\Σ}}{x}_{k-1}^i-X_{k-1}---\left(8\right)$

${\left({\mathrm{\θ}}_{k-1}^{avg\_r}\right)}_y=\frac{1}{N}\·\underset{i=1}{\overset{N}{\Σ}}{y}_{k-1}^i-Y_{k-1}---\left(9\right)$

${\mathrm{\μ}}_k=(\frac{\left|\right|{\mathrm{\θ}}_k^{bias}\left|\right|-\left|\right|{\mathrm{\θ}}_{k-1}^{bias}\left|\right|}{\mathrm{\σ}}+1)\·{\mathrm{\μ}}_{k-1}---\left(10\right).$