基于二阶级联的多元素融合定位方法、装置及电子设备

摘要

本发明实施例提供了基于二阶级联的多元素融合定位方法、装置及电子设备，该方法包括：获取用户的运动参数、所述用户所在位置的磁场参数、以及由无线节点发送的无线信号和当前场景的地图信息；根据所述运动参数及所述磁场参数，计算所述用户的行人航位推算PDR参数；根据所述无线信号，通过第一预设算法，计算出所述用户的测量位置；根据所述PDR参数及所述测量位置，通过扩展卡尔曼滤波算法，计算出所述用户的一级准确位置；根据所述PDR参数、所述一级准确位置及所述地图信息，通过粒子滤波算法，计算出所述用户的二级准确位置，作为所述用户的定位位置，可以提高定位精度。

说明书

技术领域

本发明涉及定位技术领域，特别是涉及基于二阶级联的多元素融合定位方法、装置及电子设备。

背景技术

室内定位技术自提出以来得到了广泛的关注，已经成为各国研究的热点。目前室内定位的主要技术包括WiFi(WIreless-Fidelity，无线保真)、Bluetooth(蓝牙)、RFID(Radio Frequency Identification，射频识别)、红外线、超声波等无线定位技术，以及PDR(Pedestrian dead reckoning，行人航位推算)、INS(Inertial navigation system，惯性导航系统)等自主定位技术。这些技术在办公室、家庭、工厂以及商场等生活的方方面面得到了广泛的应用。

但是现有室内定位技术在室内多遮挡、强干扰、非视距等恶劣环境下，会不可避免的出现信息丢失、出错等信息不完备现象，这就导致了现有定位技术不能够满足连续稳定低功耗的室内高精度定位的需求。例如：惯性导航系统定位技术中，惯性传感器一般包括加速度传感器、陀螺仪和磁力计，多被应用到移动终端里面。惯性传感器定位有两种方法，一种是惯性积分定位法，根据牛顿运动定律，利用三轴加速度和三轴陀螺仪的数据积分计算三维姿态、速度和位置。另一种是行人航位推算，其利用行人的位移和航向进行位置解算。惯性传感器定位可以完全不依赖外部环境，可以提供连续的定位结果，但是存在累积误差，不适合长期使用。

总之，现有室内定位技术定位精度低。

发明内容

本发明实施例的目的在于提供一种基于二阶级联的多元素融合定位方法、装置及电子设备，以实现提高定位精度。具体技术方案如下：

第一方面，本发明实施例提供了一种基于二阶级联的多元素融合定位方法，所述方法包括：

获取用户的运动参数、所述用户所在位置的磁场参数、以及由无线节点发送的无线信号和当前场景的地图信息，其中，所述运动参数包括：角速度及加速度；

根据所述运动参数及所述磁场参数，计算所述用户的行人航位推算PDR参数，其中，所述PDR参数包括：步长及航向角；

根据所述无线信号，通过第一预设算法，计算出所述用户的测量位置；

根据所述PDR参数及所述测量位置，通过扩展卡尔曼滤波算法，计算出所述用户的一级准确位置；

根据所述PDR参数、所述一级准确位置及所述地图信息，通过粒子滤波算法，计算出所述用户的二级准确位置，作为所述用户的定位位置。

可选的，所述根据所述运动参数及所述磁场参数，计算所述用户的行人航位推算PDR参数，包括：

根据所述加速度，利用经验公式，计算所述用户的步长；

以四元数作为状态向量，所述角速度和所述磁场参数作为扩展卡尔曼滤波算法的观测量，校正所述角速度，得到航向角。

可选的，所述根据所述PDR参数及所述测量位置，通过扩展卡尔曼滤波算法，计算出所述用户的一级准确位置，包括：

建立所述用户的位置的扩展卡尔曼滤波算法的状态方程；

根据所述PDR参数及所述测量位置，建立所述用户的位置的扩展卡尔曼滤波算法的测量方程；

根据所述测量方法和所述状态方程，校正所述用户的位置，得到所述用户的一级准确位置。

可选的，所述根据所述PDR参数、所述一级准确位置及所述地图信息，通过粒子滤波算法，计算出所述用户的二级准确位置，作为所述用户的定位位置，包括：

以预设位置为中心，在当前场景中均匀散布N个粒子，得到第一粒子集合，其中，N为正整数；

根据所述PDR参数，结合步态模型，变换所述第一粒子集合中各粒子的位置，得到第二粒子集合；

根据所述第二粒子集合中各粒子与所述一级准确位置间的距离，及所述地图信息，确定所述第二粒子集合中各粒子的权重；

在所述第二粒子集合中，去掉权重为0的粒子，选取权重符合预设规则的粒子进行复制，并从粒子复制后的第二粒子集合中选取N个粒子，得到第三粒子集合；

平均配置所述第三粒子集合中各粒子的权重，根据所述第三粒子集合中各粒子的权重及位置，计算所述二级准确位置，作为所述用户的定位位置。

可选的，所述根据所述PDR参数，结合步态模型，变换所述第一粒子集合中各粒子的位置，得到第二粒子集合，包括：

根据及变换所述第一粒子集合中每个粒子的位置，得到第二粒子集合；

其中，为所述第二粒子集合中第i个粒子的x轴坐标，为所述第一粒子集合中第i个粒子的x轴坐标，Lstep_k-1为所述用户上一步的步长，heading_k-1为所述用户上一步的航向角，为所述第二粒子集合中第i个粒子的x轴坐标，为所述第一粒子集合中第i个粒子的x轴坐标。

可选的，所述根据所述第二粒子集合中各粒子与所述一级准确位置间的距离，及所述地图信息，确定所述第二粒子集合中各粒子的权重，包括：

根据以下公式，计算所述第二粒子集合中各粒子的第一权重系数：

其中，x_i表示所述第二粒子集合中第i个粒子坐标，x_measure为所述一级准确位置，weight_{i_measure}为所述第二粒子集合中第i个粒子的第一权重系数；

根据所述地图信息，若所述第二粒子集合中的粒子在不可达区域，则令该粒子的第二权重系数为0，若所述第二粒子集合中的粒子在可达区域，则令该粒子的第二权重系数为1；

将所述第一权重系数与所述第二权重系数的乘积作为相应粒子的权重。

可选的，所述平均配置所述第三粒子集合中各粒子的权重，根据所述第三粒子集合中各粒子的权重及位置，计算所述二级准确位置，作为所述用户的定位，包括：

平均配置所述第三粒子集合中各粒子的权重：其中，Xⁱ为所述第三粒子集合中第i个粒子，w′_i为所述第三粒子集合中第i个粒子的权重，且w′_i＝1/N；

根据计算所述二级准确位置，作为所述用户的定位，其中，X为所述二级准确位置，x_i为所述第三粒子集合中第i个粒子的坐标。

第二方面，本发明实施例提供了一种基于二阶级联的多元素融合定位装置，所述装置包括：

数据获取模块，用于获取用户的运动参数、所述用户所在位置的磁场参数、以及由无线节点发送的无线信号和当前场景的地图信息，其中，所述运动参数包括：角速度及加速度；

PDR计算模块，用于根据所述运动参数及所述磁场参数，计算所述用户的行人航位推算PDR参数，其中，所述PDR参数包括：步长及航向角；

测量位置计算模块，用于根据所述无线信号，通过第一预设算法，计算出所述用户的测量位置；

一级计算模块，用于根据所述PDR参数及所述测量位置，通过扩展卡尔曼滤波算法，计算出所述用户的一级准确位置；

二级计算模块，用于根据所述PDR参数、所述一级准确位置及所述地图信息，通过粒子滤波算法，计算出所述用户的二级准确位置，作为所述用户的定位位置。

可选的，所述PDR计算模块，包括：

步长计算子模块，用于根据所述加速度，利用经验公式，计算所述用户的步长；

航向角计算子模块，用于以四元数作为状态向量，所述角速度和所述磁场参数作为扩展卡尔曼滤波算法的观测量，校正所述角速度，得到航向角。

可选的，所述一级计算模块，包括：

第一计算子模块，用于建立所述用户的位置的扩展卡尔曼滤波算法的状态方程；

第二计算子模块，用于根据所述PDR参数及所述测量位置，建立所述用户的位置的扩展卡尔曼滤波算法的测量方程；

第三计算子模块，用于根据所述测量方法和所述状态方程，校正所述用户的位置，得到所述用户的一级准确位置。

可选的，所述二级计算模块，包括：

第一集合确定子模块，用于以预设位置为中心，在当前场景中均匀散布N个粒子，得到第一粒子集合，其中，N为正整数；

第二集合确定子模块，用于根据所述PDR参数，结合步态模型，变换所述第一粒子集合中各粒子的位置，得到第二粒子集合；

权重配置子模块，用于根据所述第二粒子集合中各粒子与所述一级准确位置间的距离，及所述地图信息，确定所述第二粒子集合中各粒子的权重；

第三集合确定子模块，用于在所述第二粒子集合中，去掉权重为0的粒子，选取权重符合预设规则的粒子进行复制，并从粒子复制后的第二粒子集合中选取N个粒子，得到第三粒子集合；

定位位置确定子模块，用于平均配置所述第三粒子集合中各粒子的权重，根据所述第三粒子集合中各粒子的权重及位置，计算所述二级准确位置，作为所述用户的定位位置。

可选的，所述第二集合确定子模块，具体用于：

根据及变换所述第一粒子集合中每个粒子的位置，得到第二粒子集合；

其中，为所述第二粒子集合中第i个粒子的x轴坐标，为所述第一粒子集合中第i个粒子的x轴坐标，Lstep_k-1为所述用户上一步的步长，heading_k-1为所述用户上一步的航向角，为所述第二粒子集合中第i个粒子的x轴坐标，为所述第一粒子集合中第i个粒子的x轴坐标。

可选的，所述权重配置子模块，包括：

第一系数确定单元，用于根据以下公式，计算所述第二粒子集合中各粒子的第一权重系数：

其中，x_i表示所述第二粒子集合中第i个粒子坐标，x_measure为所述一级准确位置，weight_{i_measure}为所述第二粒子集合中第i个粒子的第一权重系数；

第二系数确定单元，用于根据所述地图信息，若所述第二粒子集合中的粒子在不可达区域，则令该粒子的第二权重系数为0，若所述第二粒子集合中的粒子在可达区域，则令该粒子的第二权重系数为1；

权重确定单元，用于将所述第一权重系数与所述第二权重系数的乘积作为相应粒子的权重。

可选的，所述定位位置确定子模块，包括：

权重更新单元，用于平均配置所述第三粒子集合中各粒子的权重：其中，Xⁱ为所述第三粒子集合中第i个粒子，w′_i为所述第三粒子集合中第i个粒子的权重，且w′_i＝1/N；

定位单元，用于根据计算所述二级准确位置，作为所述用户的定位位置，其中，X为所述二级准确位置，x_i为所述第三粒子集合中第i个粒子的坐标。

第三方面，本发明实施例提供了一种电子设备，包括处理器、通信接口、存储器和通信总线，其中，所述处理器，所述通信接口，所述存储器通过通信总线完成相互间的通信；

所述存储器，用于存放计算机程序；

所述处理器，用于执行所述存储器上所存放的程序时，实现上述第一方面任一所述的基于二阶级联的多元素融合定位方法。

第四方面，本发明实施例提供了一种计算机可读存储介质，所述计算机可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现上述第一方面任一所述的基于二阶级联的多元素融合定位方法。

本发明实施例提供的基于二阶级联的多元素融合定位方法、装置及电子设备，获取用户的运动参数、所述用户所在位置的磁场参数、以及由无线节点发送的无线信号和当前场景的地图信息；根据所述运动参数及所述磁场参数，计算所述用户的行人航位推算PDR参数；根据所述无线信号，通过第一预设算法，计算出所述用户的测量位置；根据所述PDR参数及所述测量位置，通过扩展卡尔曼滤波算法，计算出所述用户的一级准确位置；根据所述PDR参数、所述一级准确位置及所述地图信息，通过粒子滤波算法，计算出所述用户的二级准确位置，作为所述用户的定位位置。通过扩展卡尔曼滤波算法和粒子滤波算法，有效结合了PDR定位与无线定位的优点，同时引入地图信息辅助定位，大大提高了定位精度。当然，实施本发明的任一产品或方法必不一定需要同时达到以上所述的所有优点。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例的基于二阶级联的多元素融合定位方法的一种流程示意图；

图2为本发明实施例的利用蓝牙信号计算用户测定位置的示意图；

图3为本发明实施例的基于二阶级联的多元素融合定位方法的另一种流程示意图；

图4为本发明实施例的基于二阶级联的多元素融合定位装置的一种示意图；

图5为本发明实施例的基于二阶级联的多元素融合定位装置的另一种示意图；

图6为本发明实施例的电子设备的示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

近年来，室内定位技术应用越来越广泛。典型的室内定位技术有蓝牙、WiFi、RFID(Radio Frequency Identification，射频识别)、惯性传感器定位。

蓝牙定位技术是在蓝牙局域网的基础上构建的。在室内安装适当的局域网接入点后，对于任意进入该局域网的蓝牙用户，都可以与蓝牙节点通信，并可以通过信号传输模型估算蓝牙接入点与标签之间的距离，最后通过几何测量法估算标签的位置。蓝牙定位也可以通过指纹匹配的算法来实现，但是此种方法并不常用。蓝牙定位技术的优点是功耗低，成本低、设备体积小、易集成、易普及，但是蓝牙的传输距离短，稳定性较差，定位结果不连续，有跳变。

无线局域网是一种可以广泛应用于复杂环境下的大范围定位、监测和追踪任务的信息平台。WiFi定位算法主要有两种，一种是指纹库算法，在离线阶段建立WiFi指纹库，在线阶段通过KNN(k-NearestNeighbor，k最邻近分类算法)、WKNN(Weighted K-nearestNeighbor，加权k最邻近分类算法)等算法定位用户的位置；另一种是根据信号传播模型或者直接利用各种测距方法得到用户到无线热点的距离，然后根据三边定位算法、到达时间法等定位用户的位置。WiFi定位技术的优点是定位范围广、成本低、通信能力强，但是很容易受到其他信号的干扰，从而影响定位精度，而且定位器的功耗也较高。

RFID定位技术利用射频信号进行非接触式双向通信交换数据已达到识别和定位的目的。RFID定位技术的优点是传输范围大、成本低，但是作用距离短，定位精度易受环境影响，而且射频信号不具有通信的能力。

现有定位技术无法实现高精度连续稳定的定位。由于惯性传感器定位累积误差严重，WIFI、蓝牙、RFID等无线定位技术易受环境影响，在室内多遮挡、强干扰、非视距等恶劣环境下，往往会出现信息丢失、出错等信息不完备现象，从而降低定位精度。

有鉴于此，本发明实施例提供了一种基于二阶级联的多元素融合定位方法，参见图1，该方法包括：

S101，获取用户的运动参数、上述用户所在位置的磁场参数、以及由无线节点发送的无线信号和当前场景的地图信息，其中，上述运动参数包括：角速度及加速度。

本发明实施例的基于二阶级联的多元素融合定位方法可以通过定位系统实现，定位系统为任意能够实现本发明实施例的基于二阶级联的多元素融合定位方法的系统。例如：

定位系统可以为一种设备，包括：处理器、存储器、通信接口和总线；处理器、存储器和通信接口通过总线连接并完成相互间的通信；存储器存储可执行程序代码；处理器通过读取存储器中存储的可执行程序代码来运行与可执行程序代码对应的程序，以用于执行本发明实施例的基于二阶级联的多元素融合定位方法。

定位系统还可以为一种应用程序，用于在运行时执行本发明实施例的基于二阶级联的多元素融合定位方法。

定位系统还可以为一种存储介质，用于存储可执行代码，可执行代码用于执行本发明实施例的基于二阶级联的多元素融合定位方法。

定位系统获取用户的运动参数，运动参数包括：角速度及加速度，例如定位系统可以通过三轴加速度计获取用户的加速度，通过三轴陀螺仪获取用户的角速度。定位系统获取用户所在位置的磁场参数，磁场参数可以包括磁场强度和磁场方向，例如定位系统通过三轴磁力计获取用户所在位置的磁场参数。定位系统获取由无线节点发送的无线信号，例如，定位系统通过蓝牙设备获取由蓝牙节点发送的蓝牙信号。定位系统获取当前场景的地图信息，例如定位系统通过相关地图软件获取当前场景的地图信息。

S102，根据上述运动参数及上述磁场参数，计算上述用户的行人航位推算PDR参数，其中，上述PDR参数包括：步长及航向角。

定位系统根据用户的加速度、角速度及磁场参数通过预设PDR算法计算步长及航向角。预设PDR算法为任意的PDR算法，可以根据实际情况自行设定。

S103，根据上述无线信号，通过第一预设算法，计算出上述用户的测量位置。

无线信号可以为蓝牙信号、WIFI信号或射频信号。相应的第一预设算法为各种无线信号对应的定位算法。定位系统将根据无线信号确定的位置作为测量位置。

S104，根据上述PDR参数及上述测量位置，通过扩展卡尔曼滤波算法，计算出上述用户的一级准确位置。

定位系统根据PDR参数，建立用户的位置的EKF(Extended Kalman Filter，扩展卡尔曼滤波)算法的状态方程，根据测量位置建立所述用户的位置的EKF算法的观测方程，通过状态方程及测量位置确定用户的一级准确位置。

具体的，根据上述PDR参数及上述测量位置，通过扩展卡尔曼滤波算法，计算出上述用户的一级准确位置，包括：

步骤一，建立上述用户的位置的扩展卡尔曼滤波算法的状态方程。

状态方程：

状态向量：X＝(x y Lstep heading)^T

其中，x为用户在x轴方向的位置，y为用户在y轴方向的位置，Lstep为步长，heading为航行角，x轴与y轴是指二维平面直角坐标系中的两个轴，二维平面直角坐标可以根据实际情况自行设定，例如x轴为自西向东方向，y轴为自南向北方向。

状态转移方程如下：

转移矩阵：

其中，X_k表示第k步的状态向量，f(x_k-1)表示转移函数，X_k-1表示第k-1步的状态向量，W_k表示状态噪声。

步骤二，根据上述PDR参数及上述测量位置，建立上述用户的位置的扩展卡尔曼滤波算法的测量方程。

测量方程：

测量向量：Z＝(x^bluettooth>bluetooth>pdr>pdr)^T

测量方程：Z_k＝h(x_k)+V_k

测量矩阵：

其中，x^bluettooth为测量位置的x轴坐标，y^bluetooth为测量位置的y轴坐标，Lstep^pdr为PDR参数中的步长，heading^pdr为PDR参数中的航行角，V_k是测量噪声，测量矩阵H_k可以是单位矩阵，x_k是第k时刻的估计值。

步骤三，根据上述测量方法和上述状态方程，校正上述用户的位置，得到上述用户的一级准确位置。

在进行更新校正之前，会初始化P，即P₀(初始时刻的P)是已知的，例如可以取nI，n不为0，I为单位矩阵，用户的初始值X₀＝(x₀,y₀)可以设置为用户最初的测量位置。根据P₀、F_t可以计算出P₁，以此类推。利用实际测量值来校正经上一步预测得到的状态先验估计值，得到用户的一级准确位置。

其中，表示第k步的先验误差协方差矩阵，P_k-1表示第k-1步的后验误差协方差矩阵，Q表示过程噪声协方差矩阵，K_k表示第k步的卡尔曼滤波增益，R_k表示第k步测量噪声的协方差矩阵，X_k表示第k步的后验估计矩阵，表示第k步的先验估计矩阵，Z_k表示第k时刻的测量矩阵，P_k表示第k步的后验误差协方差矩阵。

在本发明实施例中，通过EKF算法计算用户的一级准确位置，为后续定位提供了支持。

S105，根据上述PDR参数、上述一级准确位置及上述地图信息，通过粒子滤波算法，计算出上述用户的二级准确位置，作为上述用户的定位位置。

定位系统利用PDR参数建立表征用户位置的粒子集合，根据一级准确位置及地图信息确定各粒子的权重，根据权重对粒子集合中的粒子进行过滤及复制，并得到用户的二级准确位置，作为用户的定位位置。

具体的，根据上述PDR参数、上述一级准确位置及上述地图信息，通过粒子滤波算法，计算出上述用户的二级准确位置，作为上述用户的定位位置，包括：

步骤一，以预设位置为中心，在当前场景中均匀散布N个粒子，得到第一粒子集合，其中，N为正整数。

预设位置可以为用户上一步的定位，可以将通过无线信号测得的用户的初始位置作为预设初始点X₀＝(x₀，y₀)，作为PF算法的初始点，以X₀为中心在空间均匀分散布N个粒子，每个粒子赋予相同的权重，即1/N，将得到一个粒子集

步骤二，根据上述PDR参数，结合步态模型，变换上述第一粒子集合中各粒子的位置，得到第二粒子集合。

利用步态模型计算第一粒子集合中各粒子的下一步的位置，将下一步位置的所有粒子作为第二粒子集合。

步骤三，根据上述第二粒子集合中各粒子与上述一级准确位置间的距离，及上述地图信息，确定上述第二粒子集合中各粒子的权重。

粒子的权重w_i主要由两部分组成，第一部分是根据该粒子与一级准确位置之间的距离来计算的权重weight_{i_measure}，第二部分是根据地图信息来计算的该粒子的权重weight_{i_map}，根据地图信息计算粒子权重：如果粒子穿墙weight_{i_map}为0，如果粒子未穿墙weight_{i_map}为1。

步骤四，在上述第二粒子集合中，去掉权重为0的粒子，选取权重符合预设规则的粒子进行复制，并从粒子复制后的第二粒子集合中选取N个粒子，得到第三粒子集合。

归一化第二粒子集合中的各粒子：采用重要性重采样的原理，复制权重满足条件的粒子，并从中重新抽样N个粒子得到第三粒子集合。

步骤五，平均配置上述第三粒子集合中各粒子的权重，根据上述第三粒子集合中各粒子的权重及位置，计算上述二级准确位置，作为上述用户的定位位置。

在本发明实施例中，通过无线信号、PDR参数及地图信息融合的定位算法，结合二阶级联的EKF算法和PF(Particle Filter，粒子滤波)算法，第一级利用信号传播模型计算测量位置，作为EKF的观测量辅助PDR定位，从而得到一级准确位置；第二级利用一级准确位置及地图信息计算粒子滤波的权重，最终得到用户的定位位置，通过EKF算法有效减少了定位过程中的误差，同时通过PF算法，引入地图信息辅助定位并且有效结合了PDR定位与无线定位的优点，从而实现高精度、稳定连续的室内定位。

可选的，根据上述运动参数及上述磁场参数，计算上述用户的行人航位推算PDR参数，包括：

步骤一，根据上述加速度，利用经验公式，计算用户的步长。

通过计算步长；

其中，

Lstep_k为用户第k步的步长；ρ为步长校准系数，d_real为用户行走的真实距离，d_estimated为预设的估计距离；acc_kmax为第k步的加速度模值的最大值，acc_kmin为第k步的加速度模值的最小值；acc_k为第k步的加速度，accx_k为第k步x轴方向的加速度，accy_k为第k步y轴方向的加速度，accz_k为第k步z轴方向的加速度。x轴、y轴与z轴是指三维空间直角坐标系中的三个轴，三维空间直角坐标可以根据实际情况自行设定，例如x轴为自西向东方向，y轴为自南向北方向，z轴为自下向上方向。

步骤二，以四元数作为状态向量，上述角速度和上述磁场参数作为扩展卡尔曼滤波算法的观测量，校正上述角速度，得到航向角。

航向角的计算利用了基于四元数的EKF算法，以四元数作为状态向量，加速度和磁场参数作为EKF的观测量，校正测得的角速度，最终计算出较准确的航向角。基于四元数的EKF算法具体的步骤如下：

状态方程：

状态向量为四元数：Q＝(q₁>2>3>4)，

状态方程：

转移矩阵：

其中，

Q(t)表示第t时刻的状态量，Q(t-1)表示第t-1时刻的状态向量，I是单位矩阵，w(t)为状态噪声，F_t表示转移矩阵，表示角速度向量，表示三轴角速度，T_s表示系统更新时间。

观测方程：

观测向量为三轴加速度和三轴磁场强度，由于加速度计和磁力计的读数均是以大地坐标系(n系)为参考系的，所以需要利用基于四元数的旋转矩阵将其转化到体坐标系(b系)，体坐标系可以为三维空间直角坐标系。

观测向量：

测量矩阵：

其中，acc^b是在b系下的加速度，mag^b是b系下的磁场强度，h(Q(t))是测量方程，v(t)是测量噪声，为旋转矩阵，g为当地的重力加速度，表示当地y轴磁场强度，表示当地z轴磁场强度，表示微分算子，将测量方程线性化。

更新校正：

P_t^-＝F_tP_t-1F_t^T+V_t

K_t＝P_t^-F_t^T(F_tP_t^-F_t^T+R_t)^-1

P_t＝(I-K_tH_t)P_t^-

其中，P_t^-表示第t时刻的先验误差协方差矩阵，P_t-1表示第t-1时刻的后验误差协方差矩阵，V_t表示第t时刻的过程噪声协方差矩阵，K_t表示第t时刻的卡尔曼滤波增益，R_t表示第t时刻测量噪声的协方差矩阵，Q_t表示第t时刻的后验估计矩阵，表示第t时刻的先验估计矩阵，Z_t表示第t时刻的测量矩阵，P_t表示第t时刻的后验误差协方差矩阵。

在进行更新校正之前，会初始化P，即P₀(初始时刻的P)是已知的，例如可以取nI，n不为0，I为单位矩阵，四元数的初始值可以取[0>T。根据P₀、F_t可以计算出P₁，以此类推。四元数是EKF的状态向量，每一步更新都会得到每一步的四元数，每一步的航向角的计算就是依据每一步更新的四元数计算出来的。

航向角计算：

上述的EKF算法不断更新，可以迭代出每个时刻的最优状态向量，航向角与四元数状态向量的关系如下：

在本发明实施例中，给出了步长与航向角的计算方法，通过EKF算法计算航向角，测得的航向角更加准确。

可选的，根据上述无线信号，通过第一预设算法，计算出上述用户的测量位置，包括：

根据由蓝牙节点发送的蓝牙信号，通过蓝牙定位算法，计算出上述用户的测量位置。

在离线阶段，存储部署蓝牙节点的编号及蓝牙节点的位置，当用户处于实际环境下，用户端接收蓝牙信号，根据蓝牙信号传播模型可以计算出各个蓝牙节点到用户之间的距离。如图2所示，通过筛选，找出信号强度最大的三个蓝牙节点BS1、BS2及BS3，以三个蓝牙节点为中心，以测量得到的蓝牙节点到用户之间的距离为半径画三个圆，理论上，三个圆应该交于一点，即用户位置x。但是在实际应用中，由于室内多径及非视距等因素的干扰，三个圆很难交于一点，而是会有x1、x2及x3三个交点，可以利用最小二乘法计算用户最优的测量位置。

蓝牙信号传播模型：RSSI＝A-10λ(log₁₀d)。

其中，RSSI(Received Signal Strength Indication，接收信号强度指示)为用户接收到的蓝牙节点的信号强度；λ为信号传输常数，与实际环境有关；d为蓝牙接入点与用户之间的距离；A为用户距离蓝牙接入点1m时的蓝牙节点的信号强度值。通过实际多次测量取平均值，可以得到A和λ的值。

根据信号传播模型，可通过信号强度反演出距离模型：

d＝10^{(abs(RSSI-A)/10·λ)}

最小二乘法原理：

求解方程：BX＝Y，其中，X为未知向量，B为转移函数，Y为已知向量。

最小二乘法的目的是将误差降到最小，得到最优解。最小二乘法的求解公式如下：

X＝(B^TB)^-1B^TY

根据最小二乘法的原理求解下面方程，得到最优的用户测量位置。

其中，(x₁,y₁,z₁),(x₂,y₂,z₂)，(x₃,y₃,z₃)分别为三个蓝牙节点的位置坐标；d₁,d₂,d₃分别为三个蓝牙节点到用户之间的距离，d₁,d₂,d₃是根据蓝牙信号传播模型计算得到。

在本发明实施例中，以蓝牙节点发送的蓝牙信号为无线信号，计算用户的测量位置，能够在利用蓝牙信号传输通信数据的同时，获取用户的测量位置。

可选的，根据上述PDR参数，结合步态模型，变换上述第一粒子集合中各粒子的位置，得到第二粒子集合，包括：

根据及变换上述第一粒子集合中每个粒子的位置，得到第二粒子集合。

其中，为上述第二粒子集合中第i个粒子的x轴坐标，为上述第一粒子集合中第i个粒子的x轴坐标，Lstep_k-1为上述用户上一步的步长，heading_k-1为上述用户上一步的航向角，为上述第二粒子集合中第i个粒子的x轴坐标，为上述第一粒子集合中第i个粒子的x轴坐标。

在本发明实施例中，给出了第二粒子集合的确定方法，为计算用户定位提供了支持。

可选的，上述根据上述第二粒子集合中各粒子与上述一级准确位置间的距离，及上述地图信息，确定上述第二粒子集合中各粒子的权重，包括：

步骤一，根据以下公式，计算上述第二粒子集合中各粒子的第一权重系数：

其中，x_i表示上述第二粒子集合中第i个粒子坐标，x_measure为上述一级准确位置，weight_{i_measure}为上述第二粒子集合中第i个粒子的第一权重系数。

步骤二，根据上述地图信息，若上述第二粒子集合中的粒子在不可达区域，则令该粒子的第二权重系数为0，若上述第二粒子集合中的粒子在可达区域，则令该粒子的第二权重系数为1。

第二权重系数具体可以表示为：

其中，weight_{i_map}第二粒子集合中第i个粒子的第二权重系数。

步骤三，将上述第一权重系数与上述第二权重系数的乘积作为相应粒子的权重。

粒子的权重可以表示为：

w_i＝weight_{i_measure}·weight_{i_map}

其中，w_i为第二粒子集合中第i个粒子的权重。

在本发明实施例中，给出了第二粒子集合中各粒子权重的确定方法，结合了地图信息与一级准确位置，后续确定的定位位置更加准确。

可选的，平均配置上述第三粒子集合中各粒子的权重，根据上述第三粒子集合中各粒子的权重及位置，计算上述二级准确位置，作为上述用户的定位，包括：

步骤一，平均配置上述第三粒子集合中各粒子的权重：其中，Xⁱ为上述第三粒子集合中第i个粒子，w′_i为上述第三粒子集合中第i个粒子的权重，且w′_i＝1/N。

步骤二，根据计算上述二级准确位置，作为上述用户的定位，其中，X为上述二级准确位置，x_i为上述第三粒子集合中第i个粒子的坐标。

在本发明实施例中，通过第三粒子集合的确定用户的定位位置，用户定位更加准确。

参见图3，图3为本发明实施例的基于二阶级联的多元素融合定位方法的另一种流程示意图，包括：

S301，获取蓝牙信号。

本发明实施例的基于二阶级联的多元素融合定位方法可以应用于包括蓝牙功能的终端，例如智能手机，该终端通过蓝牙功能获取蓝牙信号，例如蓝牙通信信号，在进行正常通信的同时，方便定位。

S302，根据蓝牙信号的传播模型，结合最小二乘法计算用户的蓝牙测量位置。

终端存储部署蓝牙节点的编号及蓝牙节点的位置，当用户处于实际环境下，用户端接收蓝牙信号，根据蓝牙信号传播模型可以计算出各个蓝牙节点到用户之间的距离。通过筛选，找出信号强度最大的三个蓝牙节点，以这个三个蓝牙节点为中心，以测量得到的蓝牙节点到用户之间的距离为半径画三个圆，理论上，三个圆应该交于一点。但是在实际应用中，由于室内多径及非视距等因素的干扰，三个圆很难交于一点，可以利用最小二乘法计算最优的用户的测量位置。

S303，获取用户的加速度、角速度及磁场参数。

终端可以通过三轴加速度计获取用户的加速度，通过三轴陀螺仪获取用户的角速度，通过三轴磁力计获取用户所在位置的磁场参数，包括磁场强度和磁场方向。

S304，通过步长模型计算用户的步长，通过基于四元数的EKF算法确定用户的航向角。

根据上述加速度，利用经验公式，计算上述用户的步长；以四元数作为状态向量，上述角速度和上述磁场参数作为扩展卡尔曼滤波算法的观测量，校正上述角速度，得到航向角。

S305，根据用户的蓝牙测量位置、步长及航向角，通过EKF算法计算用户的一级准确位置。

建立上述用户的位置的扩展卡尔曼滤波算法的状态方程；根据上述PDR参数及上述测量位置，建立上述用户的位置的扩展卡尔曼滤波算法的测量方程；根据上述测量方法和上述状态方程，校正上述用户的位置，得到上述用户的一级准确位置。

S306，获取用户所在的当前场景的地图信息。

定位系统获取当前场景的地图信息，例如定位系统通过相关地图软件获取当前场景的地图信息。

S307，根据一级准确位置、地图信息、步长及航向角，通过PF算法计算用户的二级准确位置。

以预设位置为中心，在当前场景中均匀散布N个粒子，得到第一粒子集合；根据上述PDR参数，结合步态模型，变换上述第一粒子集合中各粒子的位置，得到第二粒子集合；根据上述第二粒子集合中各粒子与上述一级准确位置间的距离，及上述地图信息，确定上述第二粒子集合中各粒子的权重；在上述第二粒子集合中，去掉权重为0的粒子，选取权重符合预设规则的粒子进行复制，并从粒子复制后的第二粒子集合中选取N个粒子，得到第三粒子集合；平均配置上述第三粒子集合中各粒子的权重，根据上述第三粒子集合中各粒子的权重及位置，计算上述二级准确位置，作为上述用户的定位位置。

S308，输出二级准确位置，作为定位的用户位置。

在本发明实施例中，各步骤的序号并不是对各步骤执行顺序的具体限定，上一级的步骤(图3中箭头前的步骤)只需在其对应的下级步骤(图3中相应箭头后的步骤)之前执行即可。

在本发明实施例中，结合无线技术和惯性传感器联合定位，并加入地图信息辅助校正，以实现高精度、连续、稳定、低功耗的定位。相比于WiFi、蓝牙定位技术精度更高，功耗更低；相比于RFID定位技术，蓝牙系统易于集成在智能手机端，使用方便；相比于UWB定位技术，蓝牙设备费用更低，更易于实现。本发明实施例的基于二阶级联的多元素融合定位方法，能够实现高精度、连续稳定、低功耗的定位。

参见图4，图4为本发明实施例的基于二阶级联的多元素融合定位装置的一种示意图，包括：

数据采集模块401，用于获取用户的加速度、角速度、用户所在位置的磁场参数、以及由蓝牙节点发送的蓝牙通信信号和当前场景的地图信息。

数据采集模块401通过蓝牙功能获取蓝牙信号，通过三轴加速度计获取用户的加速度，通过三轴陀螺仪获取用户的角速度，通过三轴磁力计获取用户所在位置的磁场参数，包括磁场强度和磁场方向，通过相关地图软件获取当前场景的地图信息。

数据处理模块402，用于根据上述加速度，利用经验公式，计算用户的步长；根据蓝牙信号的传播模型，结合最小二乘法计算用户的蓝牙测量位置；并对地图信息进行预处理，确定当前场景中的可达区域及不可达区域。

融合模块403，用于融合EKF算法与PF算法，确定用户的定位位置。

融合模块403以四元数作为状态向量，角速度和磁场参数作为EKF算法的观测量，校正角速度，得到航向角。根据PDR参数及测量位置，通过EKF算法，计算出用户的一级准确位置；根据PDR参数、一级准确位置及地图信息，通过RF算法，计算出用户的二级准确位置，作为用户的定位位置。

在本发明实施例中融合蓝牙、PDR及地图信息的定位算法，基于二阶级联的扩展卡尔曼滤波和粒子滤波，第一级利用信号传播模型进行蓝牙定位，蓝牙定位结果作为EKF的观测量辅助PDR定位，从而得到用户的一级准确位置；第二级利用一级准确位置及地图信息计算粒子滤波的权重，并通过PDR更新迭代最终得到更精确的用户位置，从而实现高精度、稳定连续的室内定位。

本发明实施例还提供了一种基于二阶级联的多元素融合定位装置，参见图5，上述装置包括：

数据获取模块501，用于获取用户的运动参数、上述用户所在位置的磁场参数、以及由无线节点发送的无线信号和当前场景的地图信息，其中，上述运动参数包括：角速度及加速度；

PDR计算模块502，用于根据上述运动参数及上述磁场参数，计算上述用户的行人航位推算PDR参数，其中，上述PDR参数包括：步长及航向角；

测量位置计算模块503，用于根据上述无线信号，通过第一预设算法，计算出上述用户的测量位置；

一级计算模块504，用于根据上述PDR参数及上述测量位置，通过扩展卡尔曼滤波算法，计算出上述用户的一级准确位置；

二级计算模块505，用于根据上述PDR参数、上述一级准确位置及上述地图信息，通过粒子滤波算法，计算出上述用户的二级准确位置，作为上述用户的定位位置。

在本发明实施例中，通过无线信号、PDR参数及地图信息融合的定位算法，结合二阶级联的EKF算法和PF(Particle Filter，粒子滤波)算法，第一级利用信号传播模型计算测量位置，作为EKF的观测量辅助PDR定位，从而得到一级准确位置；第二级利用一级准确位置及地图信息计算粒子滤波的权重，最终得到用户的定位位置，从而实现高精度、稳定连续的室内定位。

可选的，上述PDR计算模块502，包括：

步长计算子模块，用于根据上述加速度，利用经验公式，计算上述用户的步长。

航向角计算子模块，用于以四元数作为状态向量，上述角速度和上述磁场参数作为扩展卡尔曼滤波算法的观测量，校正上述角速度，得到航向角。

在本发明实施例中，给出了步长与航向角的计算方法，通过EKF算法计算航向角，测得的航向角更加准确。

可选的，上述一级计算模块504，包括：

第一计算子模块，用于建立上述用户的位置的扩展卡尔曼滤波算法的状态方程；

第二计算子模块，用于根据上述PDR参数及上述测量位置，建立上述用户的位置的扩展卡尔曼滤波算法的测量方程；

第三计算子模块，用于根据上述测量方法和上述状态方程，校正上述用户的位置，得到上述用户的一级准确位置。

在本发明实施例中，以蓝牙节点发送的蓝牙信号为无线信号，计算用户的测量位置，能够在利用蓝牙信号传输通信数据的同时，获取用户的测量位置。

可选的，上述二级计算模块505，包括：

第一集合确定子模块，用于以预设位置为中心，在当前场景中均匀散布N个粒子，得到第一粒子集合，其中，N为正整数；

第二集合确定子模块，用于根据上述PDR参数，结合步态模型，变换上述第一粒子集合中各粒子的位置，得到第二粒子集合；

权重配置子模块，用于根据上述第二粒子集合中各粒子与上述一级准确位置间的距离，及上述地图信息，确定上述第二粒子集合中各粒子的权重；

第三集合确定子模块，用于在上述第二粒子集合中，去掉权重为0的粒子，选取权重符合预设规则的粒子进行复制，并从粒子复制后的第二粒子集合中选取N个粒子，得到第三粒子集合；

定位位置确定子模块，用于平均配置上述第三粒子集合中各粒子的权重，根据上述第三粒子集合中各粒子的权重及位置，计算上述二级准确位置，作为上述用户的定位位置。

在本发明实施例中，通过EKF算法计算用户的一级准确位置，为后续定位提供了支持。

可选的，上述第二集合确定子模块，具体用于：

根据及变换上述第一粒子集合中每个粒子的位置，得到第二粒子集合；

其中，为上述第二粒子集合中第i个粒子的x轴坐标，为上述第一粒子集合中第i个粒子的x轴坐标，Lstep_k-1为上述用户上一步的步长，heading_k-1为上述用户上一步的航向角，为上述第二粒子集合中第i个粒子的x轴坐标，为上述第一粒子集合中第i个粒子的x轴坐标。

在本发明实施例中，给出了PF算法的具体流程，利用PF算法得到的用户定位位置更加准确。

可选的，上述权重配置子模块，包括：

第一系数确定单元，用于根据以下公式，计算上述第二粒子集合中各粒子的第一权重系数：

其中，x_i表示上述第二粒子集合中第i个粒子坐标，x_measure为上述一级准确位置，weight_{i_measure}为上述第二粒子集合中第i个粒子的第一权重系数；

第二系数确定单元，用于根据上述地图信息，若上述第二粒子集合中的粒子在不可达区域，则令该粒子的第二权重系数为0，若上述第二粒子集合中的粒子在可达区域，则令该粒子的第二权重系数为1；

权重确定单元，用于将上述第一权重系数与上述第二权重系数的乘积作为相应粒子的权重。

在本发明实施例中，给出了PF算法的具体流程，利用PF算法得到的用户定位位置更加准确。

可选的，上述定位位置确定子模块，包括：

权重更新单元，用于平均配置上述第三粒子集合中各粒子的权重：其中，Xⁱ为上述第三粒子集合中第i个粒子，w′_i为上述第三粒子集合中第i个粒子的权重，且w′_i＝1/N；

定位单元，用于根据计算上述二级准确位置，作为上述用户的定位位置，其中，X为上述二级准确位置，x_i为上述第三粒子集合中第i个粒子的坐标。

在本发明实施例中，通过第三粒子集合的确定用户的定位位置，用户定位更加准确。

本发明实施例还提供了一种电子设备，如图6所示，包括处理器601、通信接口602、存储器603和通信总线604，其中，处理器601，通信接口602，存储器603通过通信总线604完成相互间的通信，

存储器603，用于存放计算机程序；

处理器601，用于执行存储器603上所存放的程序时，实现如下步骤：

获取用户的运动参数、上述用户所在位置的磁场参数、以及由无线节点发送的无线信号和当前场景的地图信息，其中，上述运动参数包括：角速度及加速度；

根据上述运动参数及上述磁场参数，计算上述用户的行人航位推算PDR参数，其中，上述PDR参数包括：步长及航向角；

根据上述无线信号，通过第一预设算法，计算出上述用户的测量位置；

根据上述PDR参数及上述测量位置，通过扩展卡尔曼滤波算法，计算出上述用户的一级准确位置；

根据上述PDR参数、上述一级准确位置及上述地图信息，通过粒子滤波算法，计算出上述用户的二级准确位置，作为上述用户的定位位置。

在本发明实施例中，通过无线信号、PDR参数及地图信息融合的定位算法，结合二阶级联的EKF算法和PF(Particle Filter，粒子滤波)算法，第一级利用信号传播模型计算测量位置，作为EKF的观测量辅助PDR定位，从而得到一级准确位置；第二级利用一级准确位置及地图信息计算粒子滤波的权重，最终得到用户的定位位置，通过EKF算法有效减少了定位过程中的误差，同时通过PF算法，引入地图信息辅助定位并且有效结合了PDR定位与无线定位的优点，从而实现高精度、稳定连续的室内定位。

可选的，处理器601在执行存储器603上所存放的程序时还能够实现上述任一基于二阶级联的多元素融合定位方法。

上述电子设备提到的通信总线可以是外设部件互连标准(Peripheral ComponentInterconnect，PCI)总线或扩展工业标准结构(Extended Industry StandardArchitecture，EISA)总线等。该通信总线可以分为地址总线、数据总线、控制总线等。为便于表示，图中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。

通信接口用于上述电子设备与其他设备之间的通信。

存储器可以包括随机存取存储器(Random Access Memory，RAM)，也可以包括非易失性存储器(Non-Volatile Memory，NVM)，例如至少一个磁盘存储器。可选的，存储器还可以是至少一个位于远离前述处理器的存储装置。

上述的处理器可以是通用处理器，包括中央处理器(Central Processing Unit，CPU)、网络处理器(Network Processor，NP)等；还可以是数字信号处理器(Digital SignalProcessing，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。

本发明实施例提供了一种计算机可读存储介质，该计算机可读存储介质内存储有计算机程序，计算机程序被处理器执行时实现上述任一基于二阶级联的多元素融合定位方法。

在本发明实施例中，通过无线信号、PDR参数及地图信息融合的定位算法，结合二阶级联的EKF算法和PF(Particle Filter，粒子滤波)算法，第一级利用信号传播模型计算测量位置，作为EKF的观测量辅助PDR定位，从而得到一级准确位置；第二级利用一级准确位置及地图信息计算粒子滤波的权重，最终得到用户的定位位置，通过EKF算法有效减少了定位过程中的误差，同时通过PF算法，引入地图信息辅助定位并且有效结合了PDR定位与无线定位的优点，从而实现高精度、稳定连续的室内定位。

对于基于二阶级联的多元素融合定位装置/电子设备/计算机可读存储介质的实施例而言，由于其基本相似于基于二阶级联的多元素融合定位方法的实施例，所以描述的比较简单，相关之处参见基于二阶级联的多元素融合定位方法的实施例的说明即可。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

本说明书中的各个实施例均采用相关的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于系统实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。

以上所述仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等，均包含在本发明的保护范围内。