CN110657806A - 一种基于CKF、chan解算和Savitzky-Golay平滑滤波的位置解算方法 - Google Patents

Abstract

本发明属于定位领域，具体涉及一种基于CKF、chan解算和Savitzky‑Golay平滑滤波的位置解算方法，该方法按照如下方式实现，先使用TDOA测量值误差模型(1)，再通过CKF状态方程和测量方程建模，CKF流程对滤波后数据时间同步，即可完成对TDOA测量值进行处理，进而对标签位置进行解算；最后使用chan算法进行位置解算。本发明主体技术方案构思巧妙，计算方式精巧，同时位置结算过程简单，解算结果准确，应用环境友好，市场环境广阔。

Description

一种基于CKF、chan解算和Savitzky-Golay平滑滤波的位置解 算方法

技术领域：

本发明属于定位领域，具体涉及一种基于CKF、chan解算和 Savitzky-Golay平滑滤波的位置解算方法。

背景技术：

传统的定位技术，主要适用于室外空旷位置，利用同步卫星建立的定位系统进行定位，例如全球定位系统(Global Positioning System，通常简称GPS)、北斗系统等。

然而随着城市的发展，建筑的规模越来越大，人们对室内定位的需求越来越大，然而室内接收卫星信号较差，无法使用卫星定位系统。针对这一问题现有技术中出现了一些使用无线信号进行室内定位的方案。在这些方案中，利用无线信号强度随着距离增加，逐渐衰减的原理，在需要定位的室内设置无线信号发射器，由移动目标上的信号接收装置获取无线信号，将获取的无线信号强度作为参数通过信号强度衰减公式计算得出距离无线信发射器的距离，并进一步实现定位。

但是由于以上距离计算公式在近距离时较为准确，当距离变远，由于电磁波的多径效应、信号干扰、物体遮挡等影响，计算得到的距离的准确度会下降，进而导致定位精度不高。因此本发明寻求设计一种使用TDOA 定位解算方法进行定位，由于其通信次数较少，可以有效降低定位设备的功耗，所以应用广泛。本发明提供一种基于容积卡尔曼滤波(CKF)， chan位置解算，Savitzky-Golay平滑滤波的人员室内位置解算方法。能够应用于wifi、蓝牙、UWB等无线定位领域。

发明内容：

本发明的目的在于克服现有技术的缺陷，寻求设计提供一种基于 CKF、chan解算和Savitzky-Golay平滑滤波的位置解算方法，能够有效解决现有技术中所述的上述缺陷，实现精确位置解算。

为了实现上述目的，本发明涉及的一种基于CKF、chan解算和 Savitzky-Golay平滑滤波的位置解算方法通过如下技术方案实现：

本发明涉及的一种基于CKF、chan解算和Savitzky-Golay平滑滤波的位置解算方法，其详细步骤实现如下：

S1、使用TDOA测量值误差模型(1)，

其中，r_k表示当前标签距离基站的距离，r_k-1表示上一时刻标签距离基站的距离，T表示测量间隔，v为标签运动速度，a为标签运动加速度，Δr_k标签到主基站与从基站的距离差，Δr_k主，Δr_k从分别表示标签到主基站和从基站距离，Δt_ij为信号到达基站的实际时间差，

为理想到达时间差，μ为测量传感器误差和环境误差，符合

的高斯白噪声；

S2、CKF状态方程和测量方程建模

将各个从基站到主基站的达到时间差和运动速度差作为系统状态向量 X，且X表示为

X＝[Δr₀ Δr₁ Δr₂ Δv₀ Δv₁ Δv₂]^T (2)

其中Δr₀,Δr₁,Δr₂表示定位标签到定位从基站与主基站的距离差，Δv₀,Δv₁,Δv₂表示标签相对从基站和主基站的移动速度差，状态方程如下，

X_k＝FX_k-1+ΓW_k-1 (3)

其中，F为状态转移矩阵，表示为，

式中，T表示测量时间间隔，将标签运动加速度建模为系统噪声W_k-1，系数矩阵Γ为，

观测方程建模为，

Z_k＝HX_k+V_k (6)

式中，Z_k为观测量，表达式为

Z＝[Δr_m0 Δr_m1 Δr_m2 Δv_m0 Δv_m1 Δv_m2]^T (7)

H为，

V_k为测量噪声(包含测量传感器误差和环境误差)，符合N(0，R)分布；

S3、按照如下步骤实现CKF流程

a)时间更新

设定k-1时刻的状态后延概率密度

已知，通过 Cholesky分解误差协方差矩阵

选择容积点，

式中，

计算系统方程传递后的容积点，

估计k时刻状态的预测值，

b)量测更新

对P_k/k-1做Cholesky分解，

计算容积点，

通过量测方程传递容积点，

Z_i,k/k-1＝Hχ_i,k/k-1+V_k (14)

估计k时刻量测的预测值，

估计k时刻量测协方差阵和互相关协方差阵，

估计k时刻滤波增益，

K_k＝P_xz,k/k-1(P_z,k/k-1)^-1)

(17)

求k时刻状态值，

求k时刻状态误差协方差估计，

通过以上流程，再对滤波后数据时间同步，即可完成对TDOA测量值进行处理，进而对标签位置进行解算；

S4、使用chan算法进行位置解算

接收定位基站发送的TDOA测量信息后，进行CKF处理，处理后的值作为chan算法的输入量，其中，chan算法其详细处理流程如下：

设定待确定的人员位置(x，y)，定位基站的坐标(x_i，y_i)，i＝1,2…，设定(x1,y1)为主基站，其余为从基站；

通过双曲线模型获得目标的位置估计，则有：

r_i ²＝(r_i,1+r₁)²

(21)

其中，r_i为标签到基站的距离，i＝1,2…，r_i,1为从基站到主基站的距离；

对式(20)进行移项处理后平方可得：

r_i ²＝(x_i-x)²+(y_i-y)²＝K_i-2x_ix-2y_iy+x²+y² (22)

式中，i＝1,2…，

将式(21)和式(22)联立：

令i＝1，式(20)得到：

r₁ ²＝K₁-2x₁x-2y₁y+x²+y²

(24)

式(23)减式(24)得到：

设定可使用的基站为N个，结合式(25)，并且假设x、y和r₁三者之间线性无关。令z_p＝[x y]^T则有：

h＝G_az_a

(26)

式中，

以z_a为未知量，使用加权最小二乘法求解该非线性方程，即：

其中，误差矢量Ψ可近似为具有协方差矩阵的高斯随机矢量。

表达式如下，

ψ＝cBn+0.5c²n⊙n

Ψ＝E[ψψ^T]＝c² BQB

(29)

式中，c为电磁波在空气中传播速度，

为TDOA测量值的协方差矩阵，n为Hahn和Tretter 噪声向量。

再次使用加权最小二乘法进行估计，式(26)可化简为，

ψ'＝h'＝G_a'z_a' (30)

式中，

这里Ψ′为z_a′的误差矢量。Ψ′的协方差矩阵为：

ψ'＝E[ψ'ψ'^T]＝4B'cov(z_a)B'

(32)

式中，

z_a'的最小二乘估计为：

Ψ′中的B可用第一次最小而出层估计的结果，则式(34)可化简为：

最终，对待定目标位置的估计：

通过以上过程，即可得到人员位置(x，y)；

S5、使用Savitzky-Golay滤波器进行平滑处理

使用chan算法解算结果作为Savitzky-Golay滤波器(简称为S-G滤波器)的输入，其详细流程如下：

考虑一组以n＝0为中心的2M+1个数据，可以用如下的多项式来拟合它，

拟合残差为，

其中，N是多项式阶数，M为单边点数，ε越小则对原数据拟合度越高，根据费马定律，式(38)中ε对各个参数求偏导为0时，ε最小，即

化简后可得，

式(40)就是最小二乘逼近的正规方程，在此方程中需要满足n≤2M的条件；

引进矩阵A(2M+1行，N+1列)，辅助矩阵B(N+1行，N+1列)，使得，

B＝A^TA (41)

定义输入样本值向量X和系数矩阵a为，

则式(40)可以使用下式表示，

Ba＝A^TAa＝A^TX (43)

多项式的系数矩阵a可表示为，

a＝(A^TA)^-1 A^TX＝HX，H＝(A^TA)^-1 A^T

(44)

式(44)中H的第一行就为所求的卷积系数a₀的值，且S-G滤波器的卷积系数只和滤波器阶数N，单边点数M相关，与被处理何种数据无关；

由于辅助矩阵A展开后有，

当滤波器N增大时，运算量也不断增加，考虑到位置解算对实时性的要求，这里选用1阶滤波器，为了防止过拟合带来的大计算量，选用M为 3，根据式(44)和(45)即可得到系数的值，然后对chan解算的位置进行平滑。

进一步的，在实际应用中，定位基站首先测量得到标签到各个基站的时间戳，按照上述算法先进行滤波，进而得到从基站到主基站的到达时间差ΔT；然后使用chan算法进行位置解算；最后再使用Savitzky-Golay 对位置轨迹进行平滑。

本发明与现有技术相比，取得的有益效果如下：

由于实际中使用时，定位系统属于非线性系统，使用非线性滤波效果会更好。相比于EKF算法，CKF算法不需要计算Jacobian矩阵，更加容易实现，避免了截断误差；与UKF算法相比，CKF算法计算效率更高，估计精度也更好，因此也更合适应用在室内定位解算领域。

由于直接使用chan算法解算除的位置会有波动，为了定位提高精度，更好的显示定位结果，需要使用平滑滤波算法对定位结果进行平滑处理。而Savitzky-Golay平滑滤波是一种在时域内基于局域多项式最小二乘法拟合的滤波方法。该方法将高频成分平滑出去。室内定位领域，人员行走轨迹可看作平滑曲线，而由于环境因素，部分解算的点偏差较大时导致轨迹扭曲，因此适合使用Savitzky-Golay平滑滤波。综上，本发明主体技术方案构思巧妙，计算方式精巧，同时位置结算过程简单，解算结果准确，应用环境友好，市场环境广阔。

附图说明：

图1为本发明涉及的内部信息传递过程原理示意图。

图2为本发明涉及的使用四个基站解算标签位置原理示意图。

图3为本发明涉及的设定的理想运动轨迹原理示意图。

图4为本发明涉及的只使用chan算法进示意行位置解算的结果和使用CKF、chan算法和Savitzky-Golay平滑滤波的位置解算结果对比图。

图5为本发明涉及的为图3和图4两种方式定位误差对比示意图。

具体实施方式：

下面通过实施例并结合附图对本发明进一步说明。

实施例1：

本实施例涉及的一种基于CKF、chan解算和Savitzky-Golay平滑滤波的位置解算方法，其详细步骤实现如下：

S1、使用TDOA测量值误差模型(1)，

其中，r_k表示当前标签距离基站的距离，r_k-1表示上一时刻标签距离基站的距离，T表示测量间隔，v为标签运动速度，a为标签运动加速度，Δr_k标签到主基站与从基站的距离差，Δr_k主，Δr_k从分别表示标签到主基站和从基站距离，Δt_ij为信号到达基站的实际时间差，

为理想到达时间差，μ为测量传感器误差和环境误差，符合

的高斯白噪声；

S2、CKF状态方程和测量方程建模

将各个从基站到主基站的达到时间差和运动速度差作为系统状态向量 X，且X表示为

X＝[Δr₀ Δr₁ Δr₂ Δv₀ Δv₁ Δv₂]^T (2)

其中Δr₀,Δr₁,Δr₂表示定位标签到定位从基站与主基站的距离差，Δv₀,Δv₁,Δv₂表示标签相对从基站和主基站的移动速度差，状态方程如下，

X_k＝FX_k-1+ΓW_k-1 (3)

其中，F为状态转移矩阵，表示为，

式中，T表示测量时间间隔，将标签运动加速度建模为系统噪声W_k-1，系数矩阵Γ为，

观测方程建模为，

Z_k＝HX_k+V_k (6)

式中，Z_k为观测量，表达式为

Z＝[Δr_m0 Δr_m1 Δr_m2 Δv_m0 Δv_m1 Δv_m2]^T (7)

H为，

V_k为测量噪声(包含测量传感器误差和环境误差)，符合N(0，R)分布；

S3、按照如下步骤实现CKF流程

a)时间更新

设定k-1时刻的状态后延概率密度

已知，通过 Cholesky分解误差协方差矩阵

选择容积点，

式中，

计算系统方程传递后的容积点，

估计k时刻状态的预测值，

b)量测更新

对P_k/k-1做Cholesky分解，

计算容积点，

通过量测方程传递容积点，

Z_i,k/k-1＝Hχ_i,k/k-1+V_k (14)

估计k时刻量测的预测值，

估计k时刻量测协方差阵和互相关协方差阵，

估计k时刻滤波增益，

K_k＝P_xz,k/k-1(P_z,k/k-1)^-1

(17)

求k时刻状态值，

求k时刻状态误差协方差估计，

通过以上流程，再对滤波后数据时间同步，即可完成对TDOA测量值进行处理，进而对标签位置进行解算；

S4、使用chan算法进行位置解算

接收定位基站发送的TDOA测量信息后，进行CKF处理，处理后的值作为chan算法的输入量，其中，chan算法其详细处理流程如下：

设定待确定的人员位置(x，y)，定位基站的坐标(x_i，y_i)，i＝1,2…，设定(x1,y1)为主基站，其余为从基站；

通过双曲线模型获得目标的位置估计，则有：

r_i ²＝(r_i,1+r₁)²

(21)

其中，r_i为标签到基站的距离，i＝1,2…，r_i,1为从基站到主基站的距离；

对式(20)进行移项处理后平方可得：

r_i ²＝(x_i-x)²+(y_i-y)²＝K_i-2x_ix-2y_iy+x²+y² (22)

式中，i＝1,2…，

将式(21)和式(22)联立：

令i＝1，式(20)得到：

r₁ ²＝K₁-2x₁x-2y₁y+x²+y²

(24)

式(23)减式(24)得到：

设定可使用的基站为N个，结合式(25)，并且假设x、y和r₁三者之间线性无关。令z_p＝[x y]^T则有：

h＝G_az_a

(26)

式中，

以z_a为未知量，使用加权最小二乘法求解该非线性方程，即：

其中，误差矢量Ψ可近似为具有协方差矩阵的高斯随机矢量。

表达式如下，

ψ＝cBn+0.5c²n⊙n

Ψ＝E[ψψ^T]＝c²BQB

(29)

式中，c为电磁波在空气中传播速度，

为TDOA测量值的协方差矩阵，n为Hahn和Tretter 噪声向量。

再次使用加权最小二乘法进行估计，式(26)可化简为，

ψ'＝h'＝G_a'z_a' (30)

式中，

这里Ψ′为z_a′的误差矢量。Ψ′的协方差矩阵为：

ψ'＝E[ψ'ψ'^T]＝4B'cov(z_a)B'

(32)

式中，

z_a'的最小二乘估计为：

Ψ′中的B可用第一次最小而出层估计的结果，则式(34)可化简为：

最终，对待定目标位置的估计：

通过以上过程，即可得到人员位置(x，y)；

S5、使用Savitzky-Golay滤波器进行平滑处理

使用chan算法解算结果作为Savitzky-Golay滤波器(简称为S-G滤波器)的输入，其详细流程如下：

考虑一组以n＝0为中心的2M+1个数据，可以用如下的多项式来拟合它，

拟合残差为，

其中，N是多项式阶数，M为单边点数，ε越小则对原数据拟合度越高，根据费马定律，式(38)中ε对各个参数求偏导为0时，ε最小，即

化简后可得，

式(40)就是最小二乘逼近的正规方程，在此方程中需要满足n≤2M的条件；

引进矩阵A(2M+1行，N+1列)，辅助矩阵B(N+1行，N+1列)，使得，

B＝A^TA (41)

定义输入样本值向量X和系数矩阵a为，

则式(40)可以使用下式表示，

Ba＝A^TAa＝A^TX (43)

多项式的系数矩阵a可表示为，

a＝(A^TA)^-1 A^TX＝HX，H＝(A^TA)^-1 A^T

(44)

式(44)中H的第一行就为所求的卷积系数a₀的值，且S-G滤波器的卷积系数只和滤波器阶数N，单边点数M相关，与被处理何种数据无关；

由于辅助矩阵A展开后有，

当滤波器N增大时，运算量也不断增加，考虑到位置解算对实时性的要求，这里选用1阶滤波器，为了防止过拟合带来的大计算量，选用M为 3，根据式(44)和(45)即可得到系数的值，然后对chan解算的位置进行平滑。

进一步的，在实际应用中，定位基站首先测量得到标签到各个基站的时间戳，按照上述算法先进行滤波，进而得到从基站到主基站的到达时间差ΔT；然后使用chan算法进行位置解算；最后再使用Savitzky-Golay 对位置轨迹进行平滑。

实施例2：

为了验证上述算法有效性，以UWB应用为例，以1秒定位1次做了实验，测试在8m×8m的室内进行，分别测试只使用chan进行位置解算和使用CKF、chan和Savitzky-Golay平滑滤波的解算效果，受步速影响，两次测试定位次数不相同；

图3为设定的理想运动轨迹，图4为只使用chan算法进行位置解算的结果和使用CKF、chan算法和Savitzky-Golay平滑滤波的位置解算结果对比图，图5为两种方式定位误差对比图。计算得知，只使用chan解算方法定位误差均值为0.1278米，标准差0.0927，使用KF、chan算法和Savitzky-Golay平滑滤波的定位误差均值0.0592米，标准差 0.0475；由图4和图5可以看出添加CKF和Savitzky-Golay平滑滤波后，位置解算误差更小，且运动轨迹也更加平滑，从而证明了该发明的有效性。

Claims (2)

1.一种基于CKF、chan解算和Savitzky-Golay平滑滤波的位置解算方法，其特征在于具体操作步骤按照如下方式实现：

S1、使用TDOA测量值误差模型(1)，

其中，r_k表示当前标签距离基站的距离，r_k-1表示上一时刻标签距离基站的距离，T表示测量间隔，v为标签运动速度，a为标签运动加速度，Δr_k标签到主基站与从基站的距离差，Δr_k主，Δr_k从分别表示标签到主基站和从基站距离，Δt_ij为信号到达基站的实际时间差，

为理想到达时间差，μ为测量传感器误差和环境误差，符合

的高斯白噪声；

S2、CKF状态方程和测量方程建模

将各个从基站到主基站的达到时间差和运动速度差作为系统状态向量X，且X表示为

X＝[Δr₀ Δr₁ Δr₂ Δv₀ Δv₁ Δv₂]^T (2)

其中Δr₀,Δr₁,Δr₂表示定位标签到定位从基站与主基站的距离差，Δv₀,Δv₁,Δv₂表示标签相对从基站和主基站的移动速度差，状态方程如下，

X_k＝FX_k-1+ΓW_k-1 (3)

其中，F为状态转移矩阵，表示为，

式中，T表示测量时间间隔，将标签运动加速度建模为系统噪声W_k-1，系数矩阵Γ为，

观测方程建模为，

Z_k＝HX_k+V_k (6)

式中，Z_k为观测量，表达式为

Z＝[Δr_m0 Δr_m1 Δr_m2 Δv_m0 Δv_m1 Δv_m2]^T (7)

H为，

V_k为测量噪声(包含测量传感器误差和环境误差)，符合N(0，R)分布；

S3、按照如下步骤实现CKF流程

a)时间更新

设定k-1时刻的状态后延概率密度

已知，通过Cholesky分解误差协方差矩阵

选择容积点，

式中，

计算系统方程传递后的容积点，

估计k时刻状态的预测值，

b)量测更新

对P_k/k-1做Cholesky分解，

计算容积点，

通过量测方程传递容积点，

Z_i,k/k-1＝Hχ_i,k/k-1+V_k (14)

估计k时刻量测的预测值，

估计k时刻量测协方差阵和互相关协方差阵，

估计k时刻滤波增益，

K_k＝P_xz,k/k-1(P_z,k/k-1)^-1

(17)

求k时刻状态值，

求k时刻状态误差协方差估计，

通过以上流程，再对滤波后数据时间同步，即可完成对TDOA测量值进行处理，进而对标签位置进行解算；

S4、使用chan算法进行位置解算

接收定位基站发送的TDOA测量信息后，进行CKF处理，处理后的值作为chan算法的输入量，其中，chan算法其详细处理流程如下：

设定待确定的人员位置(x，y)，定位基站的坐标(x_i，y_i)，i＝1,2…，设定(x1,y1)为主基站，其余为从基站；

通过双曲线模型获得目标的位置估计，则有：

r_i ²＝(r_i,1+r₁)²

(21)

其中，r_i为标签到基站的距离，i＝1,2…，r_i,1为从基站到主基站的距离；

对式(20)进行移项处理后平方可得：

r_i ²＝(x_i-x)²+(y_i-y)²＝K_i-2x_ix-2y_iy+x²+y² (22)

式中，

将式(21)和式(22)联立：

令i＝1，式(20)得到：

r₁ ²＝K₁-2x₁x-2y₁y+x²+y²

(24)

式(23)减式(24)得到：

设定可使用的基站为N个，结合式(25)，并且假设x、y和r₁三者之间线性无关。令z_p＝[xy]^T则有：

h＝G_az_a

(26)

式中，

以z_a为未知量，使用加权最小二乘法求解该非线性方程，即：

其中，误差矢量Ψ可近似为具有协方差矩阵的高斯随机矢量。

表达式如下，

ψ＝cBn+0.5c²n⊙n

Ψ＝E[ψψ^T]＝c²BQB

(29)

式中，c为电磁波在空气中传播速度，

为TDOA测量值的协方差矩阵，n为Hahn和Tretter噪声向量。

再次使用加权最小二乘法进行估计，式(26)可化简为，

ψ'＝h'＝G_a'z_a' (30)

式中，

这里Ψ′为z_a′的误差矢量。Ψ′的协方差矩阵为：

ψ'＝E[ψ'ψ'^T]＝4B'cov(z_a)B'

(32)

式中，

B'＝diag{x⁰-x₁,y⁰-y₁,r₁ ⁰}

(33)

z_a'的最小二乘估计为：

Ψ′中的B可用第一次最小而出层估计的结果，则式(34)可化简为：

最终，对待定目标位置的估计：

通过以上过程，即可得到人员位置(x，y)；

S5、使用Savitzky-Golay滤波器进行平滑处理

使用chan算法解算结果作为Savitzky-Golay滤波器(简称为S-G滤波器)的输入，其详细流程如下：

考虑一组以n＝0为中心的2M+1个数据，可以用如下的多项式来拟合它，

拟合残差为，

其中，N是多项式阶数，M为单边点数，ε越小则对原数据拟合度越高，根据费马定律，式(38)中ε对各个参数求偏导为0时，ε最小，即

化简后可得，

式(40)就是最小二乘逼近的正规方程，在此方程中需要满足n≤2M的条件；

引进矩阵A(2M+1行，N+1列)，辅助矩阵B(N+1行，N+1列)，使得，

B＝A^TA (41)

定义输入样本值向量X和系数矩阵a为，

则式(40)可以使用下式表示，

Ba＝A^TAa＝A^TX (43)

多项式的系数矩阵a可表示为，

a＝(A^TA)^-1A^TX＝HX，H＝(A^TA)^-1A^T

(44)

式(44)中H的第一行就为所求的卷积系数a₀的值，且S-G滤波器的卷积系数只和滤波器阶数N，单边点数M相关，与被处理何种数据无关；

由于辅助矩阵A展开后有，

当滤波器N增大时，运算量也不断增加，考虑到位置解算对实时性的要求，这里选用1阶滤波器，为了防止过拟合带来的大计算量，选用M为3，根据式(44)和(45)即可得到系数的值，然后对chan解算的位置进行平滑。

2.根据权利要求1所述的一种基于CKF、chan解算和Savitzky-Golay平滑滤波的位置解算方法，其特征在于在实际应用中，定位基站首先测量得到标签到各个基站的时间戳，按照上述算法先进行滤波，进而得到从基站到主基站的到达时间差ΔT；然后使用chan算法进行位置解算；最后再使用Savitzky-Golay对位置轨迹进行平滑。

Images