[发明专利]一种融合加速度计和超宽带测距的定位方法

权利要求书

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值。