CN104197936B - 一种基于建筑物规则结构信息的室内定位方法 - Google Patents

Abstract

本发明公开了一种基于建筑物规则结构信息的室内定位方法，首先通过一般惯性传感器定位算法解算得到初级定位结果，然后根据矩形结构建筑物内的八个先验方向在粒子滤波算法框架内调整粒子权值，校正航向漂移，最后经过重采样可以得到轨迹修正后的定位结果，其中八个先验方向包含矩形四个边的朝向与四个经插值得到的辅助方向。本发明可利用建筑物的矩形规则结构对定位结果进行航向校正，显著提高了定位精度。本发明所提出的方法稳健性好，在行走轨迹复杂的条件下，仍能准确估计航向，抑制了航向漂移，提高了长时间室内定位的精确度。

Description

一种基于建筑物规则结构信息的室内定位方法

技术领域

本发明涉及基于惯性设备的室内定位和导航技术,特别是一种基于建筑物规则结构信息的抑制航向漂移方法。

背景技术

卫星导航信号在室内或者城市环境下由于遮蔽和多径现象，存在很大的定位误差，难以在这样的环境下进行较为准确的定位和导航。基于MEMS(微机电系统)的IMU(惯性测量单元)可测量目标运动数据(包括加速度与角速度)，经过惯性传感器定位算法解算后得到人员的定位结果。本发明称所有基于惯性传感器定位解算方法得到的定位结果为初级定位结果，由于器件的漂移特性，通常这类方法解算后的定位误差随时间或距离增大，特别是长时间持续定位时航向误差严重增大。

为了抑制初级定位结果的传感器误差，一般有两种途径：一种是提高传感器的精度，但传感器精度的提高会使得设备的体积、重量和成本大大增加，在很多场合不能适用；另一种是利用先验知识进行校准，从而在特定环境下通过算法对传感器误差进行抑制。传统的利用建筑物结构校正的方法包括HDE(启发式的方向校正)方法，但该方法在校正过程中方向仅离散为建筑物结构的四个方向，在弧形等不规则路径情况下或建筑物结构局部为非矩形情况下，可导致错误的方向判断，使航向错误且定位失败。

发明内容

本发明所要解决的技术问题是：针对现有技术不足，提供一种基于建筑物规则结构信息的室内定位方法，利用建筑物内部的规则结构对定位结果进行校正，抑制航向的漂移，从而提高室内定位的精确度。

为解决上述技术问题，本发明所采用的技术方案是：一种基于建筑物规则结构信息的室内定位方法，包括以下步骤：

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

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

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

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

Δθ_k＝θ_k-θ_k-1

其中，△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]}：

l_k ^[i]＝l_k+l_noise ^[i]

θ_k ^[i]＝θ_k-1 ^[i]+△θ_k+△θ_noise ^[i]

x_k ^[i]＝x_k-1 ^[i]+l_k ^[i]cos(θ_k ^[i])；

y_k ^[i]＝y_k-1 ^[i]+l_k ^[i]sin(θ_k ^[i])

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

7)根据上述步骤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]}取均值，得到航向校正后的位置：

与现有技术相比，本发明所具有的有益效果为：本发明可利用建筑物的规则结构对定位结果进行校正，显著提高了定位精度，本发明的方法稳定性较高，在行走轨迹复杂的条件下，仍能准确判断航向，提高室内定位的精确度，从而减缓或抑制了长时间持续定位结果恶化。

附图说明

图1为本发明方法流程图；

图2以人行走为例分步示意图；

图3为本发明中由建筑物结构得到的八个先验角度；

图4为某商场内不同方法定位轨迹对比图。

具体实施方式

如图1所示，本发明的定位方法包含如下几个步骤：

步骤一：测量目标运动数据，包含加速度与角速度；

步骤二：将测量得到的运动数据经过惯性传感器定位算法解算，得到每一步的位置{x_k,y_k}；

步骤三：根据粒子滤波算法，利用建筑物结构信息辅助定位，可以得到位置的粒子{x_k ^[1:N],y_k ^[1:N]}；

步骤四：对位置的粒子取均值，得到航向校正后的位置。

(1)

所述步骤二中，解算航坐标系下的位置{x_k,y_k}过程为：

步骤二(a)：确定导航坐标系，确定运动目标(如人员)的初始位置与初始朝向；

步骤二(b)：将测量得到的运动数据经过ZUPT-aided EKF(零速更新辅助的拓展卡尔曼滤波)算法解算，得到每一步在导航坐标系下的位置{x_k,y_k}。

所述步骤三中，利用建筑物结构辅助定位的粒子滤波过程为：

步骤三(a)：如图3所示，根据矩形建筑的朝向，确定矩形四个边在导航坐标系下的初始角度ψ₁、ψ₃、ψ₅与ψ₇，将角度两两进行线性插值，得到四个辅助方向的角度ψ₂、ψ₄、ψ₆与ψ₈；

步骤三(b)：如图2所示，按照人的运动模型，将当前位置{x_k,y_k}与前一步的位置{x_k-1,y_k-1}进行计算后，得到步长l_k与转向角△θ_k，

△x_k＝x_k-x_k-1

△y_k＝y_k-y_k-1

△θ_k＝θ_k-θ_k-1

其中，步长l_k表示当前步的行走距离，转向角△θ_k表明当前步与前一步前进方向的变化量，下标k表示步数；

步骤三(c)：将步长l_k与转向角△θ_k分别加上高斯噪声得到步长与转向角的噪声粒子l_noise ^[1:N]与△θ_noise ^[1:N]，噪声由p(l_r,k|l_k)和p(△θ_r,k|△θ_k)经过抽样得到

(3)

其中

σ_l ²＝c|l_k| (4)

σ_△θ ²＝d|△θ_k|

N为粒子数目，σ_l ²与σ_△θ ²表示步长与转角的噪声方差，分别与步长和转角成正比关系，比例系数为c与d，c与d取值范围均为[0,1]。

抽样过程为简单抽样，即取N个服从均值为0方差为σ_l ²的正态分布随机数，以及N个服从均值为0方差为σ_△θ ²的正态分布随机数，可作为分别服从p(l_r,k|l_k)和p(△θ_r,k|△θ_k)的噪声粒子l_noise ^[1:N]与△θ_noise ^[1:N]。

步骤三(d)：噪声粒子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]}，粒子集的更新过程(传播过程)包含

l_k ^[i]＝l_k+l_noise ^[i]

θ_k ^[i]＝θ_k-1 ^[i]+△θ_k+△θ_noise ^[i] (5)

x_k ^[i]＝x_k-1 ^[i]+l_k ^[i]cos(θ_k ^[i])

y_k ^[i]＝y_k-1 ^[i]+l_k ^[i]sin(θ_k ^[i])

上标i表示第i个粒子；

步骤三(e)：重复步骤三(d)的过程，直到将所有粒子进行更新后，可得到新的粒子集{x_k ^[1:N],y_k ^[1:N],l_k ^[1:N],θ_k ^[1:N]}；

步骤三(f)：根据步骤三(a)所确定的矩形建筑物内的八个方向，按照高斯加权更新粒子的权值，使越靠近八个先验方向的粒子权值越大；

步骤三(g)：重复步骤三(f)，直到所有粒子的权值都更新完成；

步骤三(h)：对粒子的权值进行归一化处理

步骤三(i)：重复步骤三(h)，直到得到所有粒子的归一化权值w_k ^[1:N]；

步骤三(j)：根据得到的权值w_k ^[1:N]对按照Residual Resampling(剩余重采样)算法对所有粒子进行重采样得到位置的粒子{x_k ^'[1:N],y_k ^'[1:N]}。

图4中，采用本发明校正后轨迹正确，人员行走后能够回到起点，并且由于柔性的粒子滤波校正过程使得方法能够适应弧形的轨迹。而校正前的轨迹由于方向漂移，最终轨迹发生了角度缓慢偏转，而传统HDE校正方法由于弧形路径的存在，校正过程使得方向发生大幅度偏移，最终轨迹发生错误。

Claims (1)

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}.$