一种结合建筑物结构信息与RFID标定的改进型PDR方法与流程

本发明属于惯性定位领域，具体是一种结合建筑物结构信息与rfid标定的改进型pdr算法。

背景技术：

pdr算法，即pedestriandeadreckoning，行人航迹推算算法，该算法在基于惯性原理的行人定位中使用最广泛的技术。pdr算法的原理是利用行人行走运动时产生的周期性变化的加速度特征，通过步态检测算法获得行人行走步数，通过步长估算模型解算出行人每一步的步长，结合由陀螺仪、磁力计等器件获得的行人航向，根据几何原理推算出行人位置。pdr算法的实现过程一般包括三个核心步骤：步态检测、步长估算、姿态解算。pdr算法实现的是二维位置坐标的解算，在一些行人定位系统中，增加了气压计，用以解算行人高度，实现三维位置坐标获取。步态检测、步长估算、姿态解算等算法，都存在误差，这些误差在pdr解算过程中，会持续累加，如果不对pdr算法的累积误差进行分析和处理，pdr算法的误差会持续累积，最终无法得到准备的行人位置。目前，针对pdr算法的累积误差补偿问题，学者们做出了大量研究。一方面，学者们针对pdr算法中的步态检测等几个子算法进行优化，如路永乐等(文献：路永乐,陈永炜,李瑶,李俊林,刘宇.基于mimu的自适应步态检测算法[j].压电与声光,2018,40(05):768-771+775.)利用加速度计三轴模值方差、单轴方差差别和波形相位识别4种不同的行走状态，包括前进、快跑、后退和横向行走，然后针对不同的行走状态设置自适应阈值，实现各类运动状态下的自适应步态检测，行走和跑步状态下，步态检测精度能够达到99％。米刚等(文献：米刚,田增山,金悦,etal.基于mimu和磁力计的姿态更新算法研究？[j].传感技术学报,2015(1):43-48.)提出一种基于微惯性测量单元(mimu)及磁力计信息融合的姿态解算算法，该算法能够有效地抑制陀螺的发散，实现稳定地输出高精度姿态数据。子算法上的优化，能够减小pdr算法的累积误差。此外，学者们也提出了诸多融合其他定位算法或者神经网络算法等新技术，用于抑制pdr算法的累积误差。如徐龙阳等(文献：徐龙阳,郑增威,孙霖,霍梅梅.基于神经网络的多传感器融合pdr定位方法[j].传感技术学报,2018,31(04):579-587.)结合神经网络算法，通过行人移动距离预测和微航向角融合算法，能够有效提高pdr算法的定位精度。宋宇等(文献：宋宇,喻文举,程超,etal.基于wifi、pdr、地磁相融合的室内定位研究[j].微电子学与计算机,2018(6).)通过无迹卡尔曼滤波算法将wifi定位及pdr定位的结果进行融合，做到无线定位于惯性定位技术的优势互补，提高系统定位精度。

rfid技术射频识别技术(radiofrequencyidentification,rfid)是一种利用射频信号，通过空间耦合(交变磁场或电磁场)原理，实现非接触信息传递和目标识别的技术。pdr算法因为累积误差等问题，无法长时间使用，如果利用rfid的“身份识别”功能，获得能够有效补偿pdr算法累积误差的标定数据，能够有效提高pdr算法的定位精度和使用时间。

本文针对pdr算法的累积误差问题，提出了一种结合建筑物结构信息与rfid标定的改进型pdr算法，能够使用rfid以非接触的形式获得标定数据，通过标定算法，补偿pdr算法的累积误差，实现惯性定位系统的长时间使用。

技术实现要素：

本发明旨在解决以上现有技术的问题。提出了一种能够为惯性定位系统提供精度更高的pdr算法的结合建筑物结构信息与rfid标定的改进型pdr方法。

本发明的技术方案如下：

一种结合建筑物结构信息与rfid标定的改进型pdr方法，其在传统行人航迹推算pdr算法的基础上，融合通过rfid系统得到的标定数据，补偿传统pdr算法的累积误差，具体包括步骤：

步骤1、根据定位环境中建筑物结构信息和人员行走习惯，确定标定点位置和标定点种类；

步骤2、将标定数据按照航向标定算法、步长估算模型系数k标定算法、位置标定算法、高度标定算法融合到pdr解算过程中，其中，标定点按pdr算法中的关键参数航向、步长、位置、高度分为四类：航向标定点、步长估算系数标定点、位置标定点、高度标定点，分别写入对应的标定数据：(xk',yk')、hk'、lkcorection，

进一步的，所述步骤2的航向标定算法包含如下流程：

a)通过基于扩展卡尔曼滤波的四元数姿态解算算法融合加速度计、磁力计和陀螺仪数据进行姿态解算，获得实时航向取前两秒航向数据均值得到初始航向

b)将航向标定标签写入数据后，放置于平行于或者正交于行人初始航向的走廊中；

c)根据几何原理，根据公式(7)得到触发航向标定时的四个标定航向值；

d)当行人通过航向标定标签并检测到航向标定数据时，按照公式(8)和根据基于扩展卡尔曼滤波的四元数姿态解算算法实时解算的航向计算参与pdr算法的航向φ；

其中，δ为实时航向与标准航向的允许偏差；

e)使用d)中得到的航向φ参与pdr位置解算。

进一步的，所述步骤d)使用基于扩展卡尔曼滤波的四元数姿态解算算法，将载体坐标转换中的四元数作为状态矢量，将测量的三轴加速度计值和三轴磁力计值分别进行归一化，得到ak和mk，作为观测量，构建卡尔曼滤波方程，如公式(16)：

式中，表示状态转移矩阵，ωk表示在k时刻测量得到的矢量的角速率值，wk为过程噪声矢量，ts为采样时间间隔，g、h分别表示当地归一化的重力加速度矢量和归一化的磁场强度矢量，vk、uk为测量噪声矢量，是四元数更新得到的姿态旋转矩阵；

最后，通过更新后的四元数(q0,q1,q2,q3)，得到目标航向角

进一步的，所述步骤2步长估算模型系数k标定算法包含如下步骤：

a)假设行人初始位置为点o，沿行人初始行驶方向选取点a，|oa|≥30m，在a点放置rfid标签，在标签中写入的数据为lkcorection，其中，lkcorection为的距离，即lkcorection＝|oa|；

b)行人从点o出发，沿方向直接途径点a，此过程中，行人不转向，放置于定位装置内部的rfid读卡器读取数据lkcorection，经过a点后，行人可自由运动；

c)按照公式(9)解算标定后的步长估算模型系数k'；

其中，k为步长估算系数的默认值，sli为行人第i步的估算步长，k为行人在路径上行走的步数；

d)标定后的步长估算模型系数k'替代默认系数k将参与后续所有的步长估算。

进一步的，所述步骤2的位置标定算法包含如下步骤：

a)选定位置标定标签的放置点，以行人出发点为原点，通过实地测量获得位置标定点的绝对位置坐标(xk',yk')，将该坐标写入位置标定标签，将标签放置在该点处；

b)根据pdr的核心解算公式，即公式(10)获得行人当前时刻位置坐标。

其中，lk，分别为行人第k步的步长和航向，(xk-1,yk-1)为第k-1步的位置坐标。

c)当行人通过并触发位置标定标签时，读取标签中的标定数据(xk',yk')，pdr算法最终的输出坐标为(xk',yk')，同时该坐标作为(xk-1,yk-1)，带入公式(10)参与下一次位置解算。

进一步的，所述步骤2的高度标定算法包含如下步骤：

a)获取气压计实时气压，根据公式(11)解算实时高度；

其中，p为0.5s内的气压计数据均值；

b)取开机后2秒的高度数据解算处初始高度h0，最终输出的高度值h根据公式(12)解算；

其中，δ为消除高度突跳的阈值；h为通过公式(11)实时解算的高度；u为消除高度突跳的系数。

c)将高度标定标签写入标准高度数据h'后，放在定位区域内行人大概率会通过的地方，卫生间旁的走廊；

d)触发高度标定后，读取高度标定数据h'，输出高度hk＝h'，同时该数据重新带入公式(12)，参与下一次解算。

本发明的优点及有益效果如下：

本发明所述的一种结合建筑物结构信息与rfid标定的改进型pdr算法，该算法在传统pdr算法的基础上，结合建筑物结构信息，融合通过rfid系统得到的标定数据，补偿传统pdr算法的累积误差。该算法能够为惯性定位系统提供精度更高的pdr算法，并且对惯性定位系统的硬件改动小，结合建筑物结构信息，也能有效减少标定标签的放置数量，减小对惯性定位系统自主性的破坏。

附图说明

图1是本发明提供优选实施例pdr算法的解算过程。

图2是本发明所述的一种结合建筑物结构信息与rfid标定的改进型pdr算法的算法流程图。

图3是本发明所述的一种结合建筑物结构信息与rfid标定的改进型pdr算法与传统pdr算法分别进行测试后的定点误差分析结果。

图4是本发明所述的一种结合建筑物结构信息与rfid标定的改进型pdr算法与传统pdr算法分别进行测试后的高度误差分析结果。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。

本发明解决上述技术问题的技术方案是：

pdr算法是惯性行人定位中，使用最广泛的技术。其原理利用行人行走运动时产生的周期性变化的加速度特征，通过步态检测算法获得行人行走步数k，通过步长估算模型解算出行人行走第k步的步长lk，结合由陀螺仪、磁力计等器件获得的行人航向数据通过公式(13)推算出行人第k步的位置(xk,yk)。图1给出了pdr的解算过程。

本发明所述的一种结合建筑物结构信息与rfid标定的改进型pdr算法的实现过程包括如下子算法：步态检测算法、步长估算算法、姿态解算算法、航向标定算法，步长估算模型系数k标定算法、二维位置坐标标定算法，高度标定算法。以下分别对几个关键技术进行说明。

①步态检测算法

将惯性定位单元放置于行人后腰部，行人行走时，左右脚周期性交替迈步，所采集到的加速度呈现一定的周期性变换。采集行走时的加速度数据，通过公式(14)取加速度模值，经平滑滤波后，加速度计三轴模值的输出曲线与正弦波曲线类似，呈周期性变化，单个周期内包含一个最大值amax和最小值amin。通过波峰-波谷检测，进行步态检测。行人行走时，身体抖动可能会行成伪波峰，造成步态误判，对此，设置波峰判定阈值ath，波谷判定阈值atl，单步态周期内，满足amax＞ath，amin＜atl，才进行一次计步。

式中，ax，ay，az和分别为加速度计在三个轴上的测量值，g为重力加速度。

经实验验证，该步态判定方法准确率能够达到98％以上。

②步长估算算法

行人行走过程中，每一步的步长并不是一个固定值，而是动态变化的，步长一般与行走速度、步频、身高等因素有关。国内外学者提出多种步长计算模型[8]，其中单参数经验模型[9]使用最广泛。该模型如公式(15)所示。

其中，amax，amin表示每步的加速度计模值的最大值和最小值，k为单位转换的常数。在算法的实际使用中，k值一般取真实步长与估算步长的比值。单参数经验模型计算简便，与检测波峰-波谷的步态检测算法能够很方便的结合。但使用该模型进行pdr解算时，k的取值对精度的影响很大，如何合理的获得k的取值，将在子算法步长估算模型系数k标定算法中说明。

③姿态解算算法

惯性定位系统中，常用的姿态解算算法包括欧拉角法、四元数法、方向余弦矩阵法等。欧拉角法存在万向节锁死的现象，难以实现全姿态解算；方向余弦矩阵法计算量大、效率低。四元数法运算简单，在行人定位算法中，使用广泛。本发明所述的一种结合建筑物结构信息与rfid标定的改进型pdr算法，使用基于扩展卡尔曼滤波的四元数姿态解算算法，将载体坐标转换中的四元数作为状态矢量，将测量的三轴加速度计值和三轴磁力计值分别进行归一化，得到ak和mk，作为观测量，构建卡尔曼滤波方程，如公式(16)。

式中，表示状态转移矩阵，ωk表示在k时刻测量得到的矢量的角速率值，wk为过程噪声矢量，ts为采样时间间隔，g、h分别表示当地归一化的重力加速度矢量和归一化的磁场强度矢量，vk、uk为测量噪声矢量。是四元数更新得到的姿态旋转矩阵。

最后，通过更新后的四元数(q0,q1,q2,q3)，得到目标航向角

④高度解算算法

在行人惯性定位系统中增加气压计，能够实现行人高度的获取，从而实现三维定位。本文采集气压值数据，进行均值滤波，根据公式(18)计算出相对于标准气压的高度值。

其中，p为0.5s内的气压计数据均值。

取开机后2秒的高度数据解算处初始高度h0，气压计的实时解算高度为h，最终输出的高度值根据公式(19)解算。

其中，δ为消除高度突跳的阈值。

⑤航向标定算法。该子算法包含如下流程：

a)算法开始运行时，通过基于扩展卡尔曼滤波的四元数姿态解算算法融合加速度计、磁力计和陀螺仪数据进行姿态解算，获得实时航向取前两秒航向数据均值得到初始航向

b)将航向标定标签写入数据后，放置于平行于或者正交于行人初始航向的走廊中。

c)根据几何原理，可以根据公式(20)得到触发航向标定时的四个标定航向值，

d)当行人通过航向标定标签并检测到航向标定数据时，按照公式(21)和根据基于扩展卡尔曼滤波的四元数姿态解算算法实时解算的航向计算参与pdr算法的航向φ。

其中，δ为实时航向与标准航向的允许偏差。

e)使用d)中得到的航向φ参与pdr位置解算。

⑥步长估算模型系数k标定，该子算法包含如下流程：

a)假设行人初始位置为点o，沿行人初始行驶方向选取点a，|oa|≥30m，在a点放置rfid标签，在标签中写入的数据为lkcorection，其中，lkcorection＝|oa|。

b)行人从点o出发，沿方向直接途径点a，此过程中，行人不转向，放置于定位装置内部的rfid读卡器读取数据lkcorection。经过a点后，行人可自由运动

c)按照公式(22)解算标定后的步长估算模型系数k'。

其中，k为步长估算系数的默认值，sli为行人第i步的估算步长，k为行人在路径上行走的步数。

d)标定后的步长估算模型系数k'替代默认系数k将参与后续所有的步长估算。

⑦位置标定算法，该子算法包含如下流程：

a)选定位置标定标签的放置点，以行人出发点为原点，通过实地测量获得位置标定点的绝对位置坐标(xk',yk')，将该坐标写入位置标定标签，将标签放置在该点处。

b)根据pdr的核心解算公式，即公式(23)获得行人当前时刻位置坐标。

其中，lk为行人第k步的步长，(xk-1,yk-1)为第k-1步的位置坐标。

c)当行人通过并触发位置标定标签时，读取标签中的标定数据(xk',yk')，pdr算法最终的输出坐标为(xk',yk')，同时该坐标作为(xk-1,yk-1)，带入公式(23)参与下一次位置解算。

⑧高度标定算法，该子算法包含如下流程：

a)根据④高度解算算法解算实时高度

b)将高度标定标签写入标准高度数据h'后，放在定位区域内行人大概率会通过的地方，卫生间旁的走廊等。

c)触发高度标定后，读取高度标定数据h'，输出高度hk＝h'，同时该数据重新带入公式(19)，参与下一次解算。

上述子算法并给完全独立，而是相互关联。图2给出了本发明所述的一种结合建筑物结构信息与rfid标定的改进型pdr算法的算法流程图。

为验证本发明所提出的一种结合建筑物结构信息与rfid标定的改进型pdr算法在传统pdr算法上的提升，设计四组实验，实验流程和方法如下：

选取重庆邮电大学第一教学楼，测试人员分别携带写入传统pdr算法的定位装置和写入本发明中的改进型pdr算法的设备进行测试，得到图3所示的定点误差分析结果和图4所示的高度误差分析结果。其中，实验一为行人携带写入传统pdr算法的定位装置进行测试；实验二为行人携带写入本发明中的改进型pdr算法的定位装置进行测试，但在该算法中只进行航向标定；实验三为行人携带写入本发明中的改进型pdr算法的定位装置进行测试，但在该算法中只进行位置\高度标定。实验三为行人携带写入本发明中的改进型pdr算法的定位装置进行测试，但在该算法中只进行步长估算系数标定。

在实验一中，在第四次按照预定轨迹行走时，由于航向解算错误直接造成了行人运动轨迹与标准轨迹相差甚远，固定点采集的数据误差均值达到6.68m，极大值达到了13.55m，如图3所示。同时，可以直观的从图3中看出，随着设备使用时间的延长，传统pdr算法精度逐渐降低。

实验二中，由于增加了航向标定，在整个测试过程中，并未出现航向解算错误的情况。通过航向标定，尽管并没有解决“累积误差”的问题，但有效提高了系统的定位精度，从图3中可以看出，在60分钟后测试结果中，误差较未进行标定时降低了50％。同样的结论也体现在进行步长估算系数标定的实。验四中，经过标定后，60分钟后的测试结果中，误差较未进行标定降低了67％。

实验三的定位精度均优于其他几组实验，并且，经过位置标定可以有效解决pdr算法的累积误差问题，从图3中可以看出固定点的误差也没有出现随时间增加。

结合所有实验数据，本发明所述的一种结合建筑物结构信息与rfid标定的改进型pdr算法，能够有效提高pdr算法的定位精度。在只进行航向标定和步长估算系数标定的情况下，该算法能够降低pdr算法的误差，在使用60分钟之后，该算法较传统pdr算法误差分别降低50％和67％；在进行位置\高度标定的情况下，能够有效抑制pdr算法的累积误差。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。