CN113566820B - 一种基于位置指纹和pdr算法的融合人行定位方法 - Google Patents

Abstract

本发明公开了一种基于位置指纹和PDR算法的融合人行定位方法，将参考点定位数据进行坐标转换，得到平面坐标系下定位数据，并分别利用位置指纹算法以及PDR算法分别根据转换后的平面坐标系下定位数据进行计算，得到第一定位坐标以及第二定位坐标，并计算融合判断值，判断是否满足预设融合阈值，若满足则通过扩展卡尔曼滤波以及粒子滤波分别进行一阶融合以及二阶融合，得到最终定位数据，否则直接将第一定位坐标作为最终定位坐标；本发明对室内定位数据进行融合，提高了定位精度，解决了传统位置指纹算法定位不稳定以及PDR算法无法确定初始位置、定位精度不高且存在累计误差的问题，具有成本低、定位精度高且适配性高的优点，可以广泛应用于多场景中。

Description

一种基于位置指纹和PDR算法的融合人行定位方法

技术领域

本发明涉及室内定位技术领域，具体涉及一种基于位置指纹和PDR算法的融合人行定位方法。

背景技术

近年来，随着智能手机的迅猛发展，人们对位置服务(Location Based Service,LBS)的需求不断增加，室内定位技术成为了信息技术研究的热点问题之一。由于室内复杂的环境，为了提高定位精度，一般会采用基于信号强度的方式。室内定位的方法主要包括红外线定位、超声波定位、射频(RFID)定位、LED可见光定位、蓝牙定位及Wi-Fi定位等。前四种室内定位方法均有较高的定位精度，但各有局限，信号传输距离较短、抗干扰能力差并且还需要外加设备，不方便用于室内定位。

现在智能终端内置的传感器越来越多样化，包括加速度传感器、方向传感器、陀螺仪、压力传感器等等，行人航位推算法(Pedestrian Dead Reckoning，PDR)利用加速度传感器测量行人移动的步长，利用方向传感器测得行人移动的方向，将步长和方向叠加起来，实现对行人的连续定位。这类定位技术的优点是传感器设备体积小、成本低，但是由于传感器灵敏度尚不够高，行人航位推算法在运算过程中存在累积误差，因而不适合长时间定位使用。

基于指纹的定位方法是先在各个采样点上采集指纹并存入数据库,然后将待测点的指纹同数据库中的指纹匹配，将相似度最大的指纹参考点的物理位置作为最终的定位位置。指纹定位法相对其他三种方法的优点包括：第一，它所需要的基础设施，就是目前应用最广泛的无线网络，定位对象就是智能手机终端，不需要任何额外的硬件参与，因此部署难度小、成本低廉；第二，获取信号RSSI值比获取AOA、TOA、TDOA的信息特征要相对容易，因此实现复杂度小。第三，相比行人航位推算法，指纹法没有累计误差，因而其定位精度相对较高。综上所述，基于指纹的定位技术是目前室内定位发展前景最大的一种定位方法。

位置指纹定位是基于蓝牙传播信号强度的室内定位方法，所谓位置指纹是指定位环境中所有的参考点与各个AP信号强度之间存在着一个唯一对应的映射关系，即每一个参考点有着唯一的一个蓝牙信号强度向量，像人的指纹一样有着自己的独特性。通过预先采集各个采样点接收到的AP信号的RSSI值，将其映射成参考点的物理坐标。

位置指纹定位法是先在各个采样点上采集指纹并存入数据库，然后将待测点的指纹同数据库中的指纹匹配，将相似度最大的指纹参考点的物理位置作为最终的定位位置。位置指纹定位是基于iBeacon传播信号强度的室内定位方法，所谓位置指纹是指定位环境中所有的参考点与各个iBeacon信号强度之间存在着一个唯一对应的映射关系，即每一个参考点有着唯一的一个iBeacon信号强度向量，像人的指纹一样有着自己的独特性。通过预先采集各个采样点接收到的iBeacon信号的RSSI值，将其映射成参考点的物理坐标。

室内环境可能会受到障碍物的影响，例如墙壁，桌子和其他设备。结果，室内环境中的位置不同。大多数组装好的iBeacon的信号强度会受到背景的影响，并且信号的动态水平也会变化，因此，不同的位置具有不同的特征。在实际定位之前，事先储存待测点的位置特征。在线定位时，将测出的多个iBeacon的信号强度值与离线存储的位置特征信息进行匹配，计算出待测点的坐标。

现在智能终端内置的传感器越来越多样化，包括加速度传感器、方向传感器、陀螺仪、压力传感器等，这些传感器可以收集有关人体活动的一些信息，例如步行加速度，步行方向等。加速度传感器用于测量加速度，陀螺仪用于测量偏转和倾斜期间的转动加速度，方向传感器用于测量运动方向。PDR算法原理如图1所示，是一种相对位置的定位算法，其中符号N和E分别代表正北和正东方向，PDR算法利用加速度传感器测量行人移动的步长，利用方向传感器测得行人移动的方向，将步长和方向叠加起来，实现对行人的连续定位。这类定位技术的优点是传感器设备体积小、成本低，但是由于传感器灵敏度尚不够高，行人航位推算法在运算过程中存在累积误差，因而不适合长时间定位使用。

目前主流的室内定位技术存在成本过高，定位精度低等缺陷。位置指纹定位算法定位精度较高。不同于基于测距的室内定位技术，位置指纹定位算法依据指纹信息进行定位。但是由于室内环境的复杂多变，位置指纹定位算法的定位结果会出现较大的定位偏差点出现，用户体验不好。在离线阶段，位置指纹定位算法采集指纹库需要极大的采样成本。随着室内环境变动和信号衰减，还需要定期进行采样。这也造成了位置指纹定位算法不具备很好的落地，也不能进行大范围的使用。PDR算法定位精度较为稳定，定位成本只需要能够支持加速度传感器，陀螺仪传感器和地磁传感器的移动终端。满足室内定位成本低的需求，具备大规模应用的特性。但是单一的PDR算法不能够作为室内定位的主要算法，因为PDR算法的定位精度较低，不足够满足室内定位系统的精度需求。而且PDR算法无法确定初始位置，存在累积误差，需要依靠其他室内定位技术进行辅助定位。

发明内容

针对现有技术中的上述不足，本发明提供了一种基于位置指纹和PDR算法的融合人行定位方法。

为了达到上述发明目的，本发明采用的技术方案为：

一种基于位置指纹和PDR算法的融合人行定位方法，包括以下步骤：

S1、采集参考点定位数据，并进行坐标转换，得到平面坐标系下定位数据；

S2、利用位置指纹算法根据步骤S1平面坐标系下定位数据计算第一定位坐标；

S3、利用PDR算法根据步骤S1平面坐标系下定位数据计算第二定位坐标；

S4、利用当前人行运动数据与历史人行运动数据计算融合判断值，并判断融合判断值是否满足预设融合阈值，若满足则进入步骤S5，否则以第一定位坐标为最终定位坐标；

S5、利用扩展卡尔曼滤波器对步骤S2第一定位坐标以及步骤S3第二定位坐标进行一阶融合，得到一阶融合数据；

S6、获取地图信息中建筑约束条件，利用粒子滤波根据步骤S5中一阶融合数据进行二阶融合，得到最终定位坐标。

本方案具有以下有益效果：

本发明提供了一种基于位置指纹和PDR算法的融合人行定位方法，首先利用融合定位算法提高了室内定位技术的定位精度，解决了传统位置指纹算法定位结果不稳定和PDR算法无法确认初始位置，定位精度不高且存在累计误差等问题，并且通过一阶和二阶的滤波算法提高了定位结果的稳定性，让室内定位技术具有更高的应用价值和更广泛的应用场景，结合满足室内定位成本低的需求，具备大规模应用的特性，且定位精度高。

进一步地，步骤S1具体为：

从传感器上获取参考点定位数据，并通过欧拉角法进行坐标转换，得到导航坐标系到平面坐标系的转换矩阵，表示为：

其中，C为转换矩阵，θ为俯仰角，φ为航向角，γ为横滚角。

该进一步方案具有以下有益效果：

通过欧拉角法进行坐标转换，得到可供算法使用的参数坐标，为数据处理过程提供便利，奠定基础。

进一步地，所述步骤S3具体包括以下分步骤：

所述步骤S3具体包括以下分步骤：

S31、利用自适应阈值行走检测法根据步骤S1平面坐标系下定位数据检测步数；

S32、利用非线性模型根据合成加速度计算单步步长；

S33、利用步骤S32的单步步长与步骤S1平面坐标系下定位数据中方向数据，计算第二定位坐标，表示为：

其中，(x₀，y₀)为待测点的初始位置，α_k和L_k分别为第k步的方向角和步长，K为总步数，(x_k，y_k)为第二定位坐标。

该进一步方案具有以下有益效果：

采用PDR算法叠加步长与方向，实现对行人的连续定位，结合非线性步长模型，优化步长计算，为多源融合定位算法，提供数据基础。

进一步地，所述步骤S31具体包括以下分步骤：

S311、利用步骤S1平面坐标下定位数据中各方向上的加速度，计算合成加速度，表示为：

其中，

为合成加速度，a_x、a_y、a_z分别为X、Y、Z三个方向上的加速度值；

S312、采用平滑滤波法对步骤S311中合成加速度进行处理，表示为：

其中，

为平滑滤波后的合成加速度，N为滤波数据的长度，t为当前时刻，

为前t-h时刻的合成加速度值，h为时间刻度；

S313、根据当前时刻的合成加速度值判断上一时刻的合成加速度值是否处于局部峰值，若是则进入步骤S314，否则回到步骤S311；

S314、判断步骤S313中局部峰值是否处于预设峰值阈值范围，若是则进入步骤S315，否则返回步骤S311；

S315、计算当前波峰和上一波峰的时间差，并判断时间差是否处于预设时间间隔阈值范围，若是则步数加1，进入步骤S316，否则回到步骤S311；

S316、记录当前时刻和上一时刻的波峰值以及时间差，并判断当前记录次数是否处于预设次数，若是则进入步骤S317，否则回到步骤S311；

S317、计算预设次数下全部波峰以及时间差的均值，并利用该计算后的波峰均值以及时间差均值更新步骤S314中预设峰值阈值范围；

S318、判断是否满足预设停止计步条件，若满足则结束行走检测，否则回到步骤S311。

该进一步方案具有以下有益效果：

通过采用合成加速度优化加速度判断过程中周期性不明显的问题，并利用平滑滤波器去除数据中的大部分噪声，很大程度上保留了数据原本特性，构建自适应阈值行走检测法计算步数，为后续过程中数据间的融合，提高了数据的准确性。

进一步地，所述步骤S317中利用波峰均值以及时间差均值更新步骤S314中预设峰值阈值范围，更新公式为：

a_thr＝a_new-(a_max-a_thr)/2

a_max＝a_new+(a_max-a_thr)/2

其中，a_new为波峰均值，a_max为预设峰值中最大值，a_thr为预设峰值中最小值。

该进一步方案具有以下有益效果：

更新预设峰值阈值范围，提高定位精度，增强自适应性。

进一步地，所述步骤S4具体包括以下步骤：

S41、利用当前人行运动数据与历史人行运动数据计算融合判断值，表示为：

D＝(s₂-s₁)(t₂-t₁)

其中，D为融合标准，t₁为上一次利用PDR算法对行人位置校正的初始时间，s₁为初始时间下行人的运动距离，t₂为行人当前的测试时间，s₂为当前时间下行人的运动的距离；

S42、判断步骤S41中融合判断值是否满足预设融合阈值，若满足预设融合阈值，则进入步骤S5，否则以第一定位坐标为最终定位坐标。

该进一步方案具有以下有益效果：

设置阈值判断位置指纹定位数据的误差，进一步判断所得到的定位数据是否需要进一步融合优化，增加本发明的适应性。

进一步地，所述步骤S5具体包括以下分步骤：

S51、采用扩展卡尔曼滤波器计算当前状态的状态预测向量，表示为：

其中，r_k为当前状态的状态预测向量，

为位置指纹算法计算的第k-1步状态最优估计对应的位置坐标，L_k为PDR算法计算的第k步的步长，α_k为PDR算法计算的第k步的方向角，w_k为过程噪声向量；

S52、利用过程噪声矩阵以及状态转移矩阵对步骤S51中状态预测向量的协方差矩阵进行更新，更新公式表示为：

其中，P_k为更新后的协方差矩阵，

为位置指纹算法计算的历史状态最优估计的协方差矩阵，Q_k为过程噪声矩阵，Φ_k为状态转移矩阵，

为状态转移矩阵的转置。

S53、利用步骤S52中更新后的协方差矩阵计算当前状态的增益向量，表示为：

K_k＝P_kH^T[HP_kH^T+R_k]^-1

其中，K_k为当前状态的增益向量，H为观测矩阵，H^T为观测矩阵的转置，R_k为测量噪声矩阵；

S54、利用步骤S53中增益向量、步骤S51中状态预测向量以及第二定位坐标中观测向量计算当前状态的最优估计，得到一阶融合数据，表示为：

其中，

为当前状态的最优估计，z_k为PDR算法计算的当前状态的观测向量。

该进一步方案具有以下有益效果：

本发明采用扩展卡尔曼滤波器进行一阶融合，提高定位结果的稳定性，为后续二阶融合提供了初始位置信息，让室内定位技术具有广泛应用场景。

进一步地，所述步骤S6具体包括以下分步骤：

S61、采用粒子滤波将步骤S5中一阶融合数据作为初始位置，生成一组均匀分布的粒子；

S62、利用PDR算法更新位置坐标，更新公式表示为：

x_k＝x_k-1+L_k-1*sinα_k-1

y_k＝y_k-1+L_k-1*cosα_k-1

其中，x_k和y_k是第二定位坐标中当前状态坐标，x_k-1和y_k-1为第二定位坐标中历史状态坐标，L_k-1为第二定位坐标中历史状态步长，α_k-1为第二定位坐标中历史状态坐标的方向角；

S63、利用测量权重与地图权重计算步骤S61中粒子权重，表示为：

w_i＝weight_{i_measure}*weight_{i_map}

其中，w_i为第i个粒子权重，weight_{i_measure}为测量权重；weight_{i_map}为地图权重；

S64、对步骤S63中粒子权重进行归一化，得到粒子综合权重，表示为：

其中，w'_p是计算的综合权重，w_p是第p个粒子权重，P是粒子总数；

S65、对步骤S64中粒子综合权重对当前粒子进行重采样，得到最终定位坐标，表示为：

其中，X为最终融合坐标，

是第二定位坐标中当前状态重采样后第p个粒子坐标。

该进一步方案具有以下有益效果：

通过粒子波进行二阶融合，提高了定位结果的稳定性，进一步解决了传统位置指纹算法定位不稳定与PDR算法无法确定初始位置，定位精度不高存在累计误差的问题。

附图说明

图1为本发明中当前PDR算法原理图；

图2为本发明提供的一种基于位置指纹和PDR算法的融合人行定位方法步骤示意图；

图3为本发明中步骤S3的分步骤示意图；

图4为本发明中步骤S31的分步骤示意图；

图5为本发明中步骤S4的分步骤示意图；

图6为本发明中步骤S5的分步骤示意图；

图7为本发明中步骤S7的分步骤示意图；

图8为本发明中实验过程的行进路线；

图9为本发明中实验过程中改进的位置指纹定位误差、融合算法以及PDR算法的误差对比图。

具体实施方式

下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

如图2所示，本发明提供了一种基于位置指纹和PDR算法的融合人行定位方法，包括以下步骤S1-步骤S6：

S1、采集参考点定位数据，并进行坐标转换，得到平面坐标系下定位数据；

本实施例中，从传感器上获取参考点定位数据，并通过欧拉角法进行坐标转换，得到导航坐标系到平面坐标系的转换矩阵，表示为：

其中，C为转换矩阵，θ为俯仰角，φ为航向角，γ为横滚角。

实际中，从用户端的惯性传感器获取基于手机坐标系的参考点定位数据，包括位置信息，地图信息以及加速度信息等，通过欧拉角法进行坐标转换，得到平面坐标下定位数据。

欧拉角法通过绕坐标系三个坐标轴依次旋转不同角度来实现坐标系间转换，其中绕三个坐标轴X、Y、Z旋转角度分别为横滚角γ，俯仰角θ和航向角φ；

首先从步骤S1平面坐标下定位数据中选取任一坐标(x_n,y_n,z_n)为初始坐标，绕Z轴旋转角度φ，得到绕Z轴旋转后的坐标点(x'_h,y'_h,z'_h)，表示为：

其中，C_z为Z轴转换矩阵；

接着将绕Z轴旋转后的坐标点(x'_h,y'_h,z'_h)绕Y轴旋转θ角度，得到绕Y轴旋转后的坐标点(x″_h,y″_h,z″_h)，表示为：

其中，C_y为Y轴转换矩阵；

然后将绕Y轴旋转后的坐标点(x″_h,y″_h,z″_h)绕X轴旋转角度γ，得到绕X轴旋转后的坐标点(x″_h,y″_h,z″_h)，表示为：

其中，C_x为X轴转换矩阵；

最后对得到的X、Y以及Z轴转换矩阵进行处理，得到从导航坐标系到平面坐标系的转换矩阵

表示为：

S2、利用位置指纹算法对步骤S1平面坐标下定位数据进行计算，得到第一定位坐标；

本实施例中，采用传统位置定位算法计算第一定位坐标。

实际中，假设在采样阶段一共采集了m个采样点的蓝牙指纹，表示为{F₁,F₂,…,F_m}，F_i＝(rssi_i1,rssi_i2,…,rssi_ij,)，其中F_i为第i个采样点的指纹，rssi_ij为在第i个采样点指纹中第j个AP热点的信号强度。假设在每个采样点总共能接收到n个AP热点的信号。在定位阶段，待测点蓝牙指纹为(RSSI₁,RSSI₂,…,RSSI_j,…RSSI_n)，RSSI_j为接收到的第j个AP热点的信号强度。用待测点指纹与指纹数据库中的指纹之间的欧式距离来度量二者的相似度。

待测点指纹信息与第i个采样点指纹信息的加权的欧式距离为：

加权K近邻算法：

(x,y)为最后计算出的定位坐标，(x_i,y_i)为数据库中第i个符合待测点指纹信息的坐标，w_i为加权系数，d_i为待测点实时指纹信息与第i个采样点指纹信息的欧氏距离。

S3、利用PDR算法根据步骤S1平面坐标系下定位数据计算第二定位坐标；

实际中，PDR算法需要确定初始位置，本发明中采用位置指纹定位算法计算出的位置作为初始位置，在定位过程中，通过预设阈值来判断位置指纹定位结果的误差较大，因此本发明引入PDR算法计算的结果来消除误差。传统算法中，一种方式是采取时间参数，当满足一个阈值时，认为位置指纹的定位精度已经不能满足要求，需要融合PDR算法的数据。但是，当用户处于静止状态时，PDR算法定位结果不会有变化，就不需要进行校正。另一种方式是采取距离参数，但是纯距离定位没有考虑到用户运动速度的不同，因此在相同的距离在时间上可能不同，PDR算法会有累计误差，这将导致不必要的PDR算法计算结果的融合。

如图3所示，本实施例中，步骤S3具体包括以下分步骤：

S31、利用自适应阈值行走检测法根据步骤S1平面坐标系下定位数据检测步数；

如图4所示，本实施例中，步骤S31具体包括以下分步骤：

S311、利用步骤S1平面坐标下定位数据中各方向上的加速度，计算合成加速度，表示为：

其中，

为合成加速度，a_x、a_y、a_z分别为X、Y、Z三个方向上的加速度值；

实际中，由于用户在行走的过程中的姿势并不固定，所以如果只采取一个方向的加速度进行判断可能会遇到周期性不明显的情况，这样不方便进行波峰的判断。X轴、Y轴、Z轴的加速度数据均具有周期性，且Z轴周期性最明显，X轴的周期性最不明显，因此本发明中采用合成加速度作为行走检测的判断。

S312、采用平滑滤波法对步骤S311中合成加速度进行处理，表示为：

其中，

为平滑滤波后的合成加速度，N为滤波数据的长度，t为当前时刻，

为前t-h时刻的合成加速度值，h为时间刻度；

S313、根据当前时刻的合成加速度值判断上一时刻的合成加速度值是否处于局部峰值，若是则进入步骤S314，否则回到步骤S311；

S314、判断步骤S313中局部峰值是否处于预设峰值阈值范围[a_thr,a_max)，若是则进入步骤S315，否则返回步骤S311；

实际中，将合成加速度绘制成合成加速度曲线，判断曲线的峰值，满足约束条件的加速度值则记为峰值，该约束条件表示为：

其中

为t时刻的合成加速度，

为t-1时刻的合成加速度，

为t+1时刻的合成加速度。

S315、计算当前波峰和上一波峰的时间差Δt，并判断时间差Δt是否处于预设时间间隔阈值范围[T_min,T_max]，若是则步数加1，进入步骤S316，否则回到步骤S311；

S316、记录当前时刻和上一时刻的波峰值以及时间差Δt，并判断当前记录次数是否处于预设次数Z，若是则进入步骤S317，否则回到步骤S311；

S317、计算预设次数Z下全部波峰a_new以及时间差的均值Δt_new，并利用该计算后的波峰均值a_new以及时间差均值Δt_new更新步骤S314中预设峰值阈值范围[a_thr,a_max)，更新公式为：

a_thr＝a_new-(a_max-a_thr)/2

a_max＝a_new+(a_max-a_thr)/2

其中，a_new为波峰均值，a_max为预设峰值中最大值，a_thr为预设峰值中最小值；

S318、判断是否满足预设停止计步条件，若满足则结束行走检测，否则回到步骤S311。

S32、利用非线性模型根据合成加速度计算单步步长；

实际中，步长计算模型常用的模型有常数步长模型、线性步长模型、非线性步长模型和人工智能步长模型。通过经验表明，男性的平均步长约为0.75m，女性的平均步长约为0.65m。常数模型将步长设定为一个固定常数，但是因为每个人的步长已经同一个人的步长都不尽相同，所以常数模型过于简单粗糙。线性步长模型仅在特定行走频率内的拟合效果较好。综上考虑，本发明采用非线性模型。

实际中，本发明中非线性模型有两种非线性模型可以考虑，表示为：

其中，L为单步步长，l为步长系数，通过位置指纹算法在定位开始阶段获取步数距离并结合步数检测、时间检测来求解，a_max和a_min为合成加速度的峰值和谷值，

为行走一步加速度的平均值。

表示为：

其中，(x₀，y₀)为待测点的初始位置，α_k和L_k分别为第k步的方向角和步长，K为总步数，(x_k，y_k)为第二定位坐标。

S4、利用当前人行运动数据与历史人行运动数据计算融合判断值，并判断融合判断值是否满足预设融合阈值，若满足则进入步骤S5，否则以第一定位坐标为最终定位坐标；

如图5所示，本实施例中，步骤S4具体包括以下分步骤：

S41、利用当前人行运动数据与历史人行运动数据计算融合判断值，表示为：

D＝(s₂-s₁)(t₂-t₁)

其中，D为融合标准，t₁为上一次利用PDR算法对行人位置校正的初始时间，s₁为初始时间下行人的运动距离，t₂为行人当前的测试时间，s₂为当前时间下行人的运动的距离；

S42、判断步骤S41中融合判断值是否满足预设融合阈值，若满足预设融合阈值，则进入步骤S5，否则以第一定位坐标为最终定位坐标。

实际中，假设上一次利用PDR定位对行人位置校正的时间为t₁，运动的距离为s₁，行人当前的测试时间为t₂，运动的距离为s₂，由公式计算出判断融合判断值D，并判断融合判断值D与预设融合阈值的经验值Th间的大小，如果经验值Th小于融合标准D，则记融合判断值D为0，则直接以第一定位坐标为最终定位坐标，若经验值Th大于或等于融合判断值D，则记融合判断值D为1，则进入步骤S5，表示为：

S5、利用扩展卡尔曼滤波器对步骤S2第一定位坐标以及步骤S3第二定位坐标进行一阶融合，得到一阶融合数据；

如图6所示，本实施例中，步骤S5具体包括以下分步骤：

S51、采用扩展卡尔曼滤波器计算当前状态的状态预测向量，表示为：

其中，r_k为当前状态的状态预测向量，

为位置指纹算法计算的第k-1步状态最优估计对应的位置坐标，L_k为PDR算法计算的第k步的步长，α_k为PDR算法计算的第k步的方向角，w_k为过程噪声向量；

S52、利用过程噪声矩阵以及状态转移矩阵对步骤S51中状态预测向量的协方差矩阵进行更新，更新公式表示为：

其中，P_k为更新后的协方差矩阵，

为位置指纹算法计算的历史状态最优估计的协方差矩阵，Q_k为过程噪声矩阵，Φ_k为状态转移矩阵，

为状态转移矩阵的转置；

S53、利用步骤S52中更新后的协方差矩阵计算当前状态的增益向量，表示为：

K_k＝P_kH^T[HP_kH^T+R_k]^-1

其中，K_k为当前状态的增益向量，H为观测矩阵，H^T为观测矩阵的转置，R_k为测量噪声矩阵；

S54、利用步骤S53中增益向量、步骤S51中状态预测向量以及第二定位坐标中观测向量计算当前状态的最优估计，得到一阶融合数据，表示为：

其中，

为当前状态的最优估计，z_k为PDR算法计算的当前状态的观测向量。

S6、获取地图信息中建筑约束条件，利用粒子滤波根据步骤S5中一阶融合数据进行二阶融合，得到最终定位坐标。

如图7所示，本实施例中，步骤S6具体包括以下分步骤：

S61、采用粒子滤波将步骤S5中一阶融合数据作为初始位置，生成一组均匀分布的粒子；

S62、利用PDR算法更新位置坐标，更新公式表示为：

x_k＝x_k-1+L_k-1*sinα_k-1

y_k＝y_k-1+L_k-1*cosα_k-1

其中，x_k和y_k是第二定位坐标中当前状态坐标，x_k-1和y_k-1为第二定位坐标中历史状态坐标，L_k-1为第二定位坐标中历史状态步长，α_k-1为第二定位坐标中历史状态坐标的方向角；

S63、利用测量权重与地图权重计算步骤S61中粒子权重，表示为：

w_i＝weight_{i_measure}*weight_{i_map}

其中，w_i为第i个粒子权重，weight_{i_measure}为测量权重；weight_{i_map}为地图权重；

S64、对步骤S63中粒子权重进行归一化，得到粒子综合权重，表示为：

其中，w'_p是计算的综合权重，w_p是第p个粒子权重，P是粒子总数；

S65、对步骤S64中粒子综合权重对当前粒子进行重采样，得到最终定位坐标，表示为：

其中，X为最终融合坐标，

是第二定位坐标中当前状态重采样后第p个粒子坐标。

本发明提供了一种基于位置指纹和PDR算法的融合人行定位方法，并对本发明中定位精度提升效能进行了测试。

首先沿着固定的路线行迹，然后分别比较两种算法单独定位结果以及融合算法的定位效果，进行路线如图8所示，行走平均步长为0.55米，将融合定位算法阈值设置为5，分别对两种算法得到的定位结果与融合算法定位结果进行统计，并根据误差计算公式

分别计算这三种算法的定位结果误差，如图9所示，可知位置指纹定位算法定位在没有增大采样的情况下，定位误差较大，定位结果不稳定，容易出现误差较大的偏离点，步行者航位算法PDR在定位初期阶段定位误差较小，但是随着定位时间的增长，定位误差逐渐增大，本发明提出的融合定位算法定位精度相对单一的位置指纹定位算法提升了35％，并且有效解决了位置指纹定位结果不稳定和PDR算法存在累计误差的问题。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

本发明中应用了具体实施例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。

Claims (2)

1.一种基于位置指纹和PDR算法的融合人行定位方法，其特征在于，包括以下步骤：

S1、采集参考点定位数据，并进行坐标转换，得到平面坐标系下定位数据，具体为：

从传感器上获取参考点定位数据，并通过欧拉角法进行坐标转换，得到导航坐标系到平面坐标系的转换矩阵，表示为：

其中，C为转换矩阵，θ为俯仰角，φ为航向角，γ为横滚角；

S2、利用位置指纹算法根据步骤S1平面坐标系下定位数据计算第一定位坐标；

S3、利用PDR算法根据步骤S1平面坐标系下定位数据计算第二定位坐标；具体包括以下分步骤：

S31、利用自适应阈值行走检测法根据步骤S1平面坐标系下定位数据检测步数；具体包括以下分步骤：

S311、利用步骤S1平面坐标下定位数据中各方向上的加速度，计算合成加速度，表示为：

其中，

为合成加速度，a_x、a_y、a_z分别为X、Y、Z三个方向上的加速度值；

S312、采用平滑滤波法对步骤S311中合成加速度进行处理，表示为：

其中，

为平滑滤波后的合成加速度，N为滤波数据的长度，t为当前时刻，

为前t-h时刻的合成加速度值，h为时间刻度；

S313、根据当前时刻的合成加速度值判断上一时刻的合成加速度值是否处于局部峰值，若是则进入步骤S314，否则回到步骤S311；

S314、判断步骤S313中局部峰值是否处于预设峰值阈值范围，若是则进入步骤S315，否则返回步骤S311；

S315、计算当前波峰和上一波峰的时间差，并判断时间差是否处于预设时间间隔阈值范围，若是则步数加1，进入步骤S316，否则回到步骤S311；

S316、记录当前时刻和上一时刻的波峰值以及时间差，并判断当前记录次数是否处于预设次数，若是则进入步骤S317，否则回到步骤S311；

S317、计算预设次数下全部波峰以及时间差的均值，并利用该计算后的波峰均值以及时间差均值更新步骤S314中预设峰值阈值范围；其中利用波峰均值以及时间差均值更新步骤S314中预设峰值阈值范围，更新公式为：

a_thr＝a_new-(a_max-a_thr)/2

a_max＝a_new+(a_max-a_thr)/2

其中，a_new为波峰均值，a_max为预设峰值中最大值，a_thr为预设峰值中最小值；

S318、判断是否满足预设停止计步条件，若满足则结束行走检测，否则回到步骤S311；

S32、利用非线性模型根据合成加速度计算单步步长；

S33、利用步骤S32的单步步长与步骤S1平面坐标系下定位数据中方向数据，计算第二定位坐标，表示为：

其中，(x₀，y₀)为待测点的初始位置，α_k和L_k分别为第k步的方向角和步长，K为总步数，(x_k，y_k)为第二定位坐标；

S4、利用当前人行运动数据与历史人行运动数据计算融合判断值，并判断融合判断值是否满足预设融合阈值，若满足则进入步骤S5，否则以第一定位坐标为最终定位坐标；

S5、利用扩展卡尔曼滤波器对步骤S2第一定位坐标以及步骤S3第二定位坐标进行一阶融合，得到一阶融合数据；具体包括以下分步骤：

S51、采用扩展卡尔曼滤波器计算当前状态的状态预测向量，表示为：

其中，r_k为当前状态的状态预测向量，

为位置指纹算法计算的第k-1步状态最优估计对应的位置坐标，L_k为PDR算法计算的第k步的步长，α_k为PDR算法计算的第k步的方向角，w_k为过程噪声向量；

S52、利用过程噪声矩阵以及状态转移矩阵对步骤S51中状态预测向量的协方差矩阵进行更新，更新公式表示为：

其中，P_k为更新后的协方差矩阵，

为位置指纹算法计算的历史状态最优估计的协方差矩阵，Q_k为过程噪声矩阵，Φ_k为状态转移矩阵，

为状态转移矩阵的转置；

S53、利用步骤S52中更新后的协方差矩阵计算当前状态的增益向量，表示为：

K_k＝P_kH^T[HP_kH^T+R_k]^-1

其中，K_k为当前状态的增益向量，H为观测矩阵，H^T为观测矩阵的转置，R_k为测量噪声矩阵；

S54、利用步骤S53中增益向量、步骤S51中状态预测向量以及第二定位坐标中观测向量计算当前状态的最优估计，得到一阶融合数据，表示为：

其中，

为当前状态的最优估计，z_k为PDR算法计算的当前状态的观测向量；

S6、获取地图信息中建筑约束条件，利用粒子滤波根据步骤S5中一阶融合数据进行二阶融合，得到最终定位坐标；具体包括以下分步骤：

S61、采用粒子滤波将步骤S5中一阶融合数据作为初始位置，生成一组均匀分布的粒子；

S62、利用PDR算法更新位置坐标，更新公式表示为：

x_k＝x_k-1+L_k-1*sinα_k-1

y_k＝y_k-1+L_k-1*cosα_k-1

其中，x_k和y_k是第二定位坐标中当前状态坐标，x_k-1和y_k-1为第二定位坐标中历史状态坐标，L_k-1为第二定位坐标中历史状态步长，α_k-1为第二定位坐标中历史状态坐标的方向角；

S63、利用测量权重与地图权重计算步骤S61中粒子权重，表示为：

w_i＝weight_{i_measure}*weight_{i_map}

其中，w_i为第i个粒子权重，weight_{i_measure}为测量权重；weight_{i_map}为地图权重；

S64、对步骤S63中粒子权重进行归一化，得到粒子综合权重，表示为：

其中，w'_p是计算的综合权重，w_p是第p个粒子权重，P是粒子总数；

S65、对步骤S64中粒子综合权重对当前粒子进行重采样，得到最终定位坐标，表示为：

其中，X为最终融合坐标，

是第二定位坐标中当前状态重采样后第p个粒子坐标。

2.根据权利要求1所述的一种基于位置指纹和PDR算法的融合人行定位方法，其特征在于，所述步骤S4包括以下分步骤：

S41、利用当前人行运动数据与历史人行运动数据计算融合判断值，表示为：

D＝(s₂-s₁)(t₂-t₁)

其中，D为融合标准，t₁为上一次利用PDR算法对行人位置校正的初始时间，s₁为初始时间下行人的运动距离，t₂为行人当前的测试时间，s₂为当前时间下行人的运动距离；

S42、判断步骤S41中融合判断值是否满足预设融合阈值，若满足预设融合阈值，则进入步骤S5，否则以第一定位坐标为最终定位坐标。

Images