CN112556689B - 一种融合加速度计和超宽带测距的定位方法 - Google Patents

Abstract

本发明公开了一种融合加速度计和超宽带测距的定位方法，具体包括以下步骤：S1采集加速度计数据及超宽带TOF测距值，并分别对三轴加速度计数据和超宽带TOF测距值其进行滤波去噪；S2利用波峰测频法对加速计数据进行步态检测，获得步态检测结果；S3结合步骤S2的步态检测结果和超宽带TOF测距值进行步长及方向搜索，然后综合步长和方向信息得到最终的定位结果。该融合加速度计和超宽带测距的定位方法有效地提高复杂环境的定位准确性及可靠性，同时简化了设备结构，降低了成本及功耗，进而提升了用户体验。

Description

一种融合加速度计和超宽带测距的定位方法

技术领域

本发明涉及无线通信及定位领域，尤其涉及一种融合加速度计和超宽带测距的定位方法。

背景技术

随着卫星定位导航技术的发展，基于位置服务的需求越来越强烈，人们对定位结果的准确性、可靠性和连续性的要求也与日俱增。然而，在位置服务中应用最为广泛的卫星定位系统，由于信号易受遮挡，卫星信号衰减严重，在室内环境及城市复杂环境下的定位应用受到了较大的限制。室内定位作为导航定位的“最后一公里”，近年已成为研究热点，也是导航与位置服务的核心问题。目前主流的室内定位技术包括：WIFI、蓝牙、RFID、UWB、惯性导航、地磁等，但是，每种定位方法在其精度、鲁棒性、成本、适用环境等方面各不相同。目前还没有一种单一的定位技术能够同时满足定位准确、实时、适应性强、可靠性高和成本低的特点。多传感器融合定位通过将各种独立的定位信息进行融合，达到信息互补和协同工作的效果，从而提高整个系统的定位性能。

据统计，人的一生当中80％的时间是待在室内，但GPS却不能在室内运作。出行导航、智能制造、智能服务等行业也亟待人们开始重新审视室内位置的价值。室内定位技术作为打开室内位置服务大门的钥匙，近年也越来越被重视。目前比较成熟的室内定位包括有超声波定位、UWB定位、惯导定位、射频识别(RFID)定位、蓝牙定位、WIFI定位等技术。相较于其他室内无线定位技术，WIFI有其独特的优势，WIFI热点遍布城市各个角落和楼区，由于其普遍存在性，使得其部署成本较低，硬件易于安装，易与智能手机结合定位，且其覆盖范围广、定位精度较高、易于实现，迅速成为了室内定位技术的研究热点。

目前基于UWB的定位技术由于其高精度的定位性能受到越来越多的关注，UWB定位通常采用基于到达时间差(TDOA)或到达时间(TOA)的定位算法，最小定位单元至少需要布设三个或四个定位基站，若范围较大，则需要按照最小单元进行扩展，范围越大部署成本越高，妨碍了其更广泛的应用。而基于惯性导航的定位方式无需部署任何信标节点，具有全天候、低成本等特征，其中行人PDR定位方法是目前研究较多的。由于PDR定位方法只能提供相对位置信息，定位误差会随着时间而累积，故需要绝对位置信息对其进行校正，将其与其他室内定位技术相结合能实现较高精度的定位。常用的PDR与超宽带融合的定位算法中，PDR算法需要加速度计、陀螺仪以及磁力计来进行辅助计算，对硬件要求较高，同时对设备的功耗影响较大。

因此，有必要开发一种简单可控且低功耗的融合加速度计和超宽带测距的定位方法来实现高性能的室内定位。

发明内容

本发明要解决的技术问题是提供一种融合加速度计和超宽带测距的定位方法，提高室内定位的准确性和可靠性，降低了超宽带基站(UWB基站)的部署成本以及融合定位标签端的成本和功耗，同时简化了配套设备的结构。

为了解决上述技术问题，本发明采用的技术方案是：该融合加速度计和超宽带测距的定位方法，具体包括以下步骤：

S1：采集加速度计数据及超宽带TOF测距值，并分别对三轴加速度计数据和超宽带TOF测距值进行滤波去噪；

S2：利用波峰测频法对加速计数据进行步态检测，获得步态检测结果；

S3：结合步骤S2的步态检测结果和超宽带TOF测距值进行步长及方向搜索，然后综合步长和方向信息得到最终的定位结果。

采用上述技方案，利用加速度计进行步态检测，得到步态检测结果，步态检测结果中包括步伐(步态)检测结果和计步检测结果，结合超宽带(UWB)的飞行时间(TOF)测距数据进行步长及方向搜索从而得到移动目标的位置信息，该融合加速度计和超宽带测距的定位方法有效地提高复杂环境的定位准确性及可靠性，同时简化了设备结构，降低了成本及功耗，进而提升了用户体验。

作为本发明的优选技术方案，所述步骤S3中结合步骤S2的步态检测结果和超宽带TOF测距值进行步长及方向搜索的方法包括预测和更新两个步骤，具体为：

S31预测：考虑到行人步长的有界性及方向的随意性，设步长的范围为[L₁,L₂]，方向的范围为[-π,π]，将步长的范围及方向的范围进行等分，得到步长粒子集{l₁,l₂,...,l_i}∈[L₁,L₂]以及方向粒子集{θ₁,θ₂,...,θ_j}∈[-π,π]，然后根据式(1)计算得到位置粒子集{(x₁,y₁),(x₂,y₂),...,(x_n,y_n)}，其中，n＝i×j；

式(1)中，k＝1,2,...,n，(x_k,y_k)为第k步时行人的位置坐标，L_k+1为第k+1步时的步长，θ_k+1为第k+1步时行人行进方向相对于北向的夹角；

S32更新：根据所述超宽带TOF测距值d与所述位置粒子集{(x₁,y₁),(x₂,y₂),...,(x_n,y_n)}中的每个粒子(x_k,y_k)到超宽带基站的距离值的差值为err_k，计算所述位置粒子集中每个粒子的概率值P_k，最后确定最大值P_max对应的位置粒子即为此次计算定位得到的位置坐标，具体的计算方法为：

其中，(x_bs,y_bs)为超宽带基站的坐标，所述位置粒子集中的每个粒子到超宽带基站的距离值与超宽带TOF测距值d的差值分布符合均值为0，标准差为R的高斯分布。

作为本发明的优选技术方案，所述步骤S1中采用三轴加速度计进行加速度计数据的采集，并通过低通滤波去噪法对三轴加速度计数据进行滤波去噪处理；同时采用超宽带基站采集超宽带TOF测距值，并通过卡尔曼滤波KF法对超宽带TOF测距值进行滤波去噪处理。

作为本发明的优选技术方案，所述超宽带基站的数量至少为2个。为了步骤S3结合步骤S2的步态检测结果和超宽带TOF测距值进行步长及方向搜索时剔除定位模糊的解，因此，超宽带基站的数量大于2个。

作为本发明的优选技术方案，所述步骤S1中采用低通滤波去噪法对三轴加速度计数据进行滤波去噪的公式为：

a_x,f(m+1)＝(1-α)*a_x(m+1)+α*a_x(m)；

式中，a_x为原始的X轴加速度，a_x,f为低通滤波后的X轴加速度，m为采集加速度值的序列号，α为低通滤波系数，α＜＜1。

作为本发明的优选技术方案，所述步骤S1中通过卡尔曼滤波KF法对超宽带TOF测距值进行滤波去噪处理的具体步骤为：

S11初始化：X(1)＝d₁，X_KF(1)＝X(1)，k＝2,3,...,M，设定初始误差P₀，过程噪声协方差Q，观测噪声协方差R，

S12预测：

X_pre(k)＝X_KF(k-1)；

P_pre＝P₀+Q；

S13更新：

Kg＝P_pre/(P_pre+R)；

Z(k)＝d_k；

X_KF(k)＝X_pre+Kg*(Z(k)-X_pre)；

P₀＝(1-Kg)*P_pre；

d_kf(k)＝X_KF(k)；

其中，X_pre为卡尔曼滤波预测过程中对估计量的预测值，X_KF为卡尔曼滤波更新过程中对预测值修正后的估计值，P_pre为卡尔曼滤波预测过程对误差的预测值，Kg为卡尔曼增益，Z(k)为观测量，即采集的超宽带TOF值，d_kf为经过卡尔曼滤波KF法处理后的超宽带TOF值。

作为本发明的优选技术方案，所述步骤S2中利用波峰测频法对加速计数据进行步态检测的具体步骤为：

S21：为了降低传感器姿态的影响，计算整体加速度a：

式中，a_x、a_y、a_z为滤波后的三轴加速度值；

S22：滑动选取N个加速度值，判断是否存在波峰：比较N个加速度值中的最大值是否满足峰值阈值区间[1.05g，1.25g]，其中g为重力加速度，若满足峰值阈值区间则转至步骤S23，若不满足则返回步骤S21；

S23：计算该次波峰与上一次波峰之间的时间差，判读该时间差是否满足时间阈值区间[0.4,1]，排除非行走造成的异常峰值，若满足时间阈值区间则步数加一，识别正确的行走步态，若不满足则返回步骤S21。

与现有技术相比，本发明具有的有益效果为：该融合加速度计和超宽带测距的定位方法通过加速度计和超宽带测距数据的融合可得到精确的定位坐标信息，避免了陀螺仪进行航向估计时容易受携带姿态及振动等外界条件的影响，同时简化了融合设备的结构，降低了成本，减小了功耗，进而提升了用户的体验。

附图说明

图1为本发明的融合加速度计和超宽带测距的定位方法的流程图；

图2为本发明的融合加速度计和超宽带测距的定位方法的预测示意图；

图3为本发明的融合加速度计和超宽带测距的定位方法的模糊解示意图；

图4为本发明的融合加速度计和超宽带测距的定位方法的确定解示意图；

图5为本发明的融合加速度计和超宽带测距的定位方法与PDR定位结果对比图。

具体实施方式

下面将结合本发明的实施例图中的附图，对本发明实施例中的技术方案进行清楚、完整的描述。

实施例：如图1所示，该融合加速度计和超宽带测距的定位方法，具体包括以下步骤：

S1：采集加速度计数据及超宽带TOF测距值，并分别对三轴加速度计数据和超宽带TOF测距值其进行滤波去噪；

其中采用三轴加速度计进行加速度计数据的采集，并通过低通滤波去噪法对三轴加速度计数据进行滤波去噪处理；同时采用超宽带基站采集超宽带TOF测距值，并通过卡尔曼滤波KF法对超宽带TOF测距值进行滤波去噪处理；

采用低通滤波去噪法对三轴加速度计数据进行滤波去噪处理的公式为(以X轴方向为例)：

a_x,f(m+1)＝(1-α)*a_x(m+1)+α*a_x(m)；

式中，a_x为原始的X轴加速度，a_x,f为低通滤波后的X轴加速度，m为采集加速度值的序列号，α为低通滤波系数，α＜＜1；

采用卡尔曼滤波KF法对超宽带TOF测距值进行滤波去噪处理的具体步骤为：

S11初始化：X(1)＝d₁，X_KF(1)＝X(1)，k＝2,3,...,M，设定初始误差P₀，过程噪声协方差Q，观测噪声协方差R，

S12预测：

X_pre(k)＝X_KF(k-1)；

P_pre＝P₀+Q；

S13更新：

Kg＝P_pre/(P_pre+R)；

Z(k)＝d_k；

X_KF(k)＝X_pre+Kg*(Z(k)-X_pre)；

P₀＝(1-Kg)*P_pre；

d_kf(k)＝X_KF(k)；

其中，X_pre为卡尔曼滤波预测过程中对估计量的预测值，X_KF为卡尔曼滤波更新过程中对预测值修正后的估计值，P_pre为卡尔曼滤波预测过程对误差的预测值，Kg为卡尔曼增益，Z(k)为观测量，即采集的超宽带TOF值，d_kf为经过卡尔曼滤波KF法处理后的超宽带TOF值；

S2：利用波峰测频法对加速计数据进行步态检测，获得步态检测结果(含计步结果)；

利用波峰测频法对加速计数据进行步态检测的具体步骤为：

所述步骤S2中利用波峰测频法对加速计数据进行步态检测的具体步骤为：

S21：为了降低传感器姿态的影响，计算整体加速度a：

式中，a_x、a_y、a_z为滤波后的三轴加速度值；

S22：滑动选取40个加速度值，判断是否存在波峰：比较40个加速度值中的最大值是否满足峰值阈值区间[1.05g，1.25g]，其中g为重力加速度，若满足峰值阈值区间则转至步骤S23，若不满足则返回步骤S21；

S23：计算该次波峰与上一次波峰之间的时间差，判读该时间差是否满足时间阈值区间[0.4,1]，排除非行走造成的异常峰值，若满足时间阈值区间则步数加一，识别正确的行走步态，若不满足则返回步骤S21；

S3：结合步骤S2的步态检测结果(含计步结果)和超宽带TOF测距值进行步长及方向搜索，然后综合步长和方向信息得到最终的定位结果；如图3所示，只有一个超宽带基站，则在以移动端MS为圆心，以步长L为半径的圆周上会存在A、B两个点到超宽带基站BS的距离等于超宽带TOF测距值，无法确认这两个点哪一个是准确值，所以为了剔除定位模糊的解，超宽带基站的数量至少为2个才能确定唯一解，如图4所示，布设两个超宽带基站BS1和BS2，则在以移动端MS为圆心，以步长L为半径的圆周上会存在A、B两个点到超宽带基站BS1的距离等于超宽带TOF1测距值，另外还有C、D两个点到超宽带基站BS2的距离等于超宽带TOF2测距值，其中B、C两个点会重合，即可得到唯一解。

其中结合步骤S2的步态检测结果和超宽带TOF测距值进行步长及方向搜索的方法包括预测和更新两个步骤，具体为：

S31预测：如图2所示，考虑到行人步长的有界性及方向的随意性，设步长的范围为[L₁,L₂]，方向的范围为[-π,π]，将步长的范围及方向的范围进行等分，得到步长粒子集{l₁,l₂,...,l_i}∈[L₁,L₂]以及方向粒子集{θ₁,θ₂,...,θ_j}∈[-π,π]，然后根据式(1)计算得到位置粒子集{(x₁,y₁),(x₂,y₂),...,(x_n,y_n)}，其中，n＝i×j；

式(1)中，k＝1,2,...,n，(x_k,y_k)为第k步时行人的位置坐标，L_k+1为第k+1步时的步长，θ_k+1为第k+1步时行人行进方向相对于北向的夹角；

S32更新：根据所述超宽带TOF测距值d与所述位置粒子集{(x₁,y₁),(x₂,y₂),...,(x_n,y_n)}中的每个粒子(x_k,y_k)到超宽带基站的距离值的差值为err_k，计算所述位置粒子集中每个粒子的概率值P_k，最后确定最大值P_max对应的位置粒子即为此次计算定位得到的位置坐标，具体的计算方法为：

其中，(x_bs,y_bs)为超宽带基站的坐标，所述位置粒子集中的每个粒子到超宽带基站的距离值与超宽带TOF测距值d的差值分布符合均值为0，标准差为R的高斯分布。

将采用PDR算法得到的定位结果与本发明的融合加速度计和超宽带测距的定位方法得到的定位结果进行对比，如图5所示，从图5中可以看出，采用PDR算法得到的定位轨迹累积误差逐渐增大，随着测试时间的增长，轨迹越来越偏离真实轨迹，而本明的融合加速度计和超宽带测距的定位方法采用加速度计结合超宽带TOF测距值的算法能够有效抑制这种问题，保持轨迹的准确性。

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

Claims (4)

1.一种融合加速度计和超宽带测距的定位方法，其特征在于，具体包括以下步骤：

S1：采集加速度计数据及超宽带TOF测距值，并分别对三轴加速度计数据和超宽带TOF测距数据进行滤波去噪；

S2：利用波峰测频法对加速计数据进行步态检测，获得步态检测结果；

S3：结合步骤S2的步态检测结果和超宽带TOF测距值进行步长及方向搜索，然后综合步长和方向信息得到最终的定位结果；

所述步骤S3中结合步骤S2的步态检测结果和超宽带TOF测距值进行步长及方向搜索的方法包括预测和更新两个步骤，具体为：

S31预测：考虑到行人步长的有界性及方向的随意性，设步长的范围为[L₁,L₂]，方向的范围为[-π,π]，将步长的范围及方向的范围进行等分，得到步长粒子集{l₁,l₂,...,l_i}∈[L₁,L₂]以及方向粒子集{θ₁,θ₂,...,θ_j}∈[-π,π]，然后根据式(1)计算得到位置粒子集{(x₁,y₁),(x₂,y₂),...,(x_n,y_n)}，其中，n＝i×j；

式(1)中，k＝1,2,...,n，(x_k,y_k)为第k步时行人的位置坐标，L_k+1为第k+1步时的步长，θ_k+1为第k+1步时行人行进方向相对于北向的夹角；

S32更新：根据所述超宽带TOF测距值d与所述位置粒子集{(x₁,y₁),(x₂,y₂),...,(x_n,y_n)}中的每个粒子(x_k,y_k)到超宽带基站的距离值的差值为err_k，计算所述位置粒子集中每个粒子的概率值P_k，最后确定最大值P_max对应的位置粒子即为此次计算定位得到的位置坐标，具体的计算方法为：

其中，(x_bs,y_bs)为超宽带基站的坐标，所述位置粒子集中的每个粒子到超宽带基站的距离值与超宽带TOF测距值d的差值分布符合均值为0，标准差为R的高斯分布；

所述步骤S1中采用三轴加速度计进行加速度计数据的采集，并通过低通滤波去噪法对三轴加速度计数据进行滤波去噪处理；同时采用超宽带基站采集超宽带TOF测距值，并通过卡尔曼滤波KF法对超宽带TOF测距值进行滤波去噪处理；

所述步骤S1中通过卡尔曼滤波KF法对超宽带TOF测距值d_k∈{d₁,d₂,...,d_M}进行滤波去噪处理的具体步骤为：

S11初始化：X(1)＝d₁，X_KF(1)＝X(1)，k＝2,3,...,M，设定初始误差P₀，过程噪声协方差Q，观测噪声协方差R，

S12预测：

X_pre(k)＝X_KF(k-1)；

P_pre＝P₀+Q；

S13更新：

Kg＝P_pre/(P_pre+R)；

Z(k)＝d_k；

X_KF(k)＝X_pre+Kg*(Z(k)-X_pre)；

P₀＝(1-Kg)*P_pre；

d_kf(k)＝X_KF(k)；

其中，X_pre为卡尔曼滤波预测过程中对估计量的预测值，X_KF为卡尔曼滤波更新过程中对预测值修正后的估计值，P_pre为卡尔曼滤波预测过程对误差的预测值，Kg为卡尔曼增益，Z(k)为观测量，即采集的超宽带TOF值，d_kf为经过卡尔曼滤波KF法处理后的超宽带TOF值。

2.根据权利要求1所述的融合加速度计和超宽带测距的定位方法，其特征在于，所述步骤S1中采用低通滤波去噪法对三轴加速度计数据进行滤波去噪的公式为：

a_x,f(m+1)＝(1-α)*a_x(m+1)+α*a_x(m)；

式中，a_x为原始的X轴加速度，a_x,f为低通滤波后的X轴加速度，m为采集加速度值的序列号，α为低通滤波系数，α<<1。

3.根据权利要求1所述的融合加速度计和超宽带测距的定位方法，其特征在于，所述超宽带基站的数量至少为2个。

4.根据权利要求2或3所述的融合加速度计和超宽带测距的定位方法，其特征在于，所述步骤S2中利用波峰测频法对加速计数据进行步态检测的具体步骤为：

S21：为了降低传感器姿态的影响，计算整体加速度a：

式中，a_x、a_y、a_z为滤波后的三轴加速度值；

S22：滑动选取N个加速度值，判断是否存在波峰：比较N个加速度值中的最大值是否满足峰值阈值区间[1.05g，1.25g]，其中g为重力加速度，若满足峰值阈值区间则转至步骤S23，若不满足则返回步骤S21；

S23：计算该次波峰与上一次波峰之间的时间差，判读该时间差是否满足时间阈值区间[0.4,1]，排除非行走造成的异常峰值，若满足时间阈值区间则步数加一，识别正确的行走步态，若不满足则返回步骤S21。