[发明专利]一种基于建筑物规则结构信息的室内定位方法

权利要求书

1.一种基于建筑物规则结构信息的室内定位方法，包括以下步骤：

1)确定导航坐标系，确定室内运动目标的初始位置与初始朝向；

2)根据建筑结构的朝向，确定矩形建筑四个边在导航坐标系下的初始角度ψ₁、ψ₃、ψ₅与ψ₇，将所述初始角度两两进行线性插值，得到四个辅助方向的角度ψ₂、ψ₄、ψ₆与ψ₈；

3)利用惯性传感器测量目标的运动数据，并根据运动数据解算得到目标位移和角度的变化量，所述变化量逐次累加得到目标的相对方位，相对方位与初始方位相加得到初级定位结果，在所述导航坐标系下表示为{x_k,y_k}，下坐标k表示第k步；

4)在初级定位结果中，通过当前位置{x_k,y_k}与前一位置{x_k-1,y_k-1}得到目标当前位置步长l_k与转向角Δθ_k：

$\begin{array}{c}{\mathrm{\Δ\θ}}_k={\mathrm{\θ}}_k-{\mathrm{\θ}}_{k-1}\\ l_k=\sqrt{{{\mathrm{\Δx}}_k}^2+{{\mathrm{\Δy}}_k}^2}\end{array};$

其中，Δx_k＝x_k-x_k-1；Δy_k＝y_k-y_k-1；

5)将当前位置步长l_k与转向角Δθ_k分别加上高斯噪声，得到当前位置步长与转向角的噪声粒子l_noise^[1:N]与Δθ_noise^[1:N]；其中，N为粒子数目；

6)利用当前位置步长与转向角的噪声粒子l_noise^[1:N]与Δθ_noise^[1:N]和前一位置的粒子集{x_k-1^[1:N],y_k-1^[1:N],l_k-1^[1:N],θ_k-1^[1:N]}，得到新的粒子集{x_k^[1:N],y_k^[1:N],l_k^[1:N],θ_k^[1:N]}：

$\begin{array}{c}{l_k}^{\[i\]}=l_k+{l_{noise}}^{\[i\]}\\ {{\mathrm{\θ}}_k}^{\[i\]}={{\mathrm{\θ}}_{k-1}}^{\[i\]}+{\mathrm{\Δ\θ}}_k+{{\mathrm{\Δ\θ}}_{noise}}^{\[i\]}\\ {x_k}^{\[i\]}={x_{k-1}}^{\[i\]}+{l_k}^{\[i\]}\cos \left({{\mathrm{\θ}}_k}^{\[i\]}\right)\\ {y_k}^{\[i\]}={y_{k-1}}^{\[i\]}+{l_k}^{\[i\]}\sin \left({{\mathrm{\θ}}_k}^{\[i\]}\right)\end{array};$

其中，1≤i≤N，表示第i个粒子；θ_k^[i]则表示第k步时，第i个粒子的朝向；Δθ_k表示通过运动传感器计算得到的转角，即朝向变化量；Δθ_noise表示高斯噪声模型下带来的随机转角；x_k-1^[i]、y_k-1^[i]分别表示第k-1个位置下第i个粒子的横坐标和纵坐标；

7)根据上述步骤2)确定的矩形建筑内的八个方向，按照下式更新每个粒子的权值：

${W_k}^{\[i\]}=\underset{j=1}{\overset{8}{\mathrm{\Σ}}}\exp (-\frac{{({{\mathrm{\θ}}_k}^{\[i\]}-{\mathrm{\ψ}}_j)}^2}{2{\mathrm{\π\σ}}^2});$

其中，ψ_j为建筑物的引导向、σ²为选用的高斯噪声模型的方差、W_k^[i]为第i个粒子在第k步的权值；

8)对每个粒子的权值分别进行归一化处理，得到归一化权值w_k^[1:N]；

9)根据归一化权值w_k^[1:N]，按照剩余重采样方法对所有粒子进行重采样，得到位置的粒子{x_k^'[1:N],y_k^'[1:N]}；

10)对上述位置的粒子{x_k^'[1:N],y_k^'[1:N]}取均值，得到航向校正后的位置：

$\begin{array}{c}\stackrel{\‾}{x_k}=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{x_k}^{\′\[i\]}\\ \stackrel{\‾}{y_k}=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{y_k}^{\′\[i\]}\end{array}.$