[发明专利]非线性非高斯条件下基于EKF和PF的协同定位方法

摘要

本发明公开非线性非高斯条件下基于EKF和PF的协同定位方法。该方法获取目标节点与各基站的TOA原始数据，计算得到目标节点与各基站之间的距离；利用Wylie鉴别法判断是否有NLOS误差；TOA值做差得到TDOA值，重构TDOA值对应的距离差r_m1；分别使用EKF算法和PF算法估算t_k时刻目标节点的位置坐标；进行残差加权得到t_k时刻的最终估计值；对所有时刻的位置坐标进行加权平滑即可得到最终定位结果。该方法较EKF更适用于非线性非高斯的定位环境，较PF有效避免了使用不正确数据，减少了计算量。该方法有效降低NLOS误差的影响，结合EKF和PF两者的优势，同时克服两者的不足，实现比较精确的定位。

主权项

1.非线性非高斯条件下基于扩展卡尔曼滤波和粒子滤波的协同定位方法，其特征在于该方法包括以下步骤：步骤(1)、获取目标节点分别与各基站的TOA原始数据，计算得到目标节点与各基站之间的距离r_m(m＝1，2，…，M)，其中M为基站总个数；步骤(2)、利用Wylie鉴别法判断是否有NLOS误差；步骤(3)、将TOA值做差得到TDOA值，并对所有TDOA值对应的距离差r_m1进行重构；步骤(4)、使用扩展卡尔曼滤波TDOA算法估算t_k时刻目标节点的位置坐标并进行判断；步骤(5)、使用粒子滤波TDOA算法估算t_k时刻目标节点的位置坐标并进行判断；步骤(6)、对步骤(4)、(5)的两个位置坐标进行判断，残差加权得到t_k时刻的最终估计值；步骤(7)、对步骤(6)得到的所有定位坐标数据进行加权平滑，得到目标节点的最终估计位置坐标步骤(2)具体如下：将定位时刻记为t_k＝0,t₁,…,t_K，其中t_K为总定位时间，T为两个相邻时刻的间隔时间，假设目标节点与每个基站的距离测量结果通过多项式拟合来进行平滑处理，即其中，J‑1表示多项式的阶数；r_m(t_k)表示目标节点在t_k时刻与第m个基站的距离；利用最小二乘法求解出未知系数可得到平滑后的测量值为以S_m(t_k)作为真实距离参考值，计算r_m(t_k)的距离测量标准偏差，可表示为假设LOS环境下的距离测量标准偏差为σ_m，可预先根据实验现场测量获得；若则判断目标节点与基站之间是LOS传播；若则判断目标节点与基站之间是NLOS传播；步骤(3)具体如下：若存在NLOS传播，将距离测量标准偏差中最小值所对应的目标节点与对应基站的距离设为r₁，该基站作为参考基站，坐标为(x₁,y₁)；目标节点与其余基站之间的距离设为r₂,…,r_M，对应基站坐标分别设为(x₂,y₂),…,(x_M,y_M)；将TOA值转换生成TDOA值，并对所有TDOA值对应的距离差r_m1的值进行重构：若r_m和r₁均为LOS传播或NLOS传播，则r_m1＝r_m‑r₁；若r_m为NLOS传播，r₁为LOS传播，则r_m1＝(r_m‑η_NLOS)‑(r₁‑η_LOS)；其中η_LOS为目标节点与基站间为LOS传播时的平均时延对应距离；η_NLOS为目标节点与基站间为NLOS传播时平均时延对应距离；步骤(4)具体如下：通过Kalman TDOA算法计算得到目标节点在t_k时刻的理论坐标值然后根据公式(5)进行判断；若满足，则保留此次定位结果继续下一步；否则放弃下一步计算，结束此次定位过程，返回步骤(1)重新读取原始TOA数据；其中δ₁为阈值，可根据定位设备的理论精度和实际实验进行人为选定；步骤(5)通过粒子滤波TDOA算法计算得到目标节点在t_k时刻的理论坐标值具体如下：5.1建立目标节点运动的状态方程和观测方程如下所示：X_tk＝F·X_t(k‑1)+S·W_t(k‑1)                    (6)Z_tk＝f(X_tk)+V_tk                        (7)式中，F为状态转移矩阵，见公式(8)；S为干扰转移矩阵，见公式(8)；W_t(k‑1)和V_tk分别为过程噪声和观测噪声；Z_tk为目标节点在t_k时刻与所有基站的TDOA观测值转换得到的距离差；为目标节点在时刻t_k的状态信息，X_t(k‑1)为目标节点在时刻t_k‑1的状态信息，其中为t_k时刻目标节点的位置坐标，为t_k时刻目标节点的速度；5.2根据目标节点运动的状态方程和观测方程，建立似然概率密度函数，见公式(9)：其中，为目标节点在t_k时刻与第m个基站的TDOA预测值转换得到的距离差，为目标节点在t_k时刻与第m个基站的TDOA观测值转换得到的距离差，σ_v为观测噪声V_tk的方差；表示t_k时刻第i个粒子的状态信息，i＝1,2,…，N，N为粒子总数；5.3通过5.1和5.2得到了状态方程和观测方程以及似然概率密度函数之后，结合粒子滤波TDOA算法滤波过程，得到定位结果更新步骤如下：a)初始化:t_k＝0由先验分布p(x₀)产生粒子群所有粒子权值为1/N；其中，p(x₀)由目标节点已知的初始状态信息结合状态方程得到；b)t_k＝t₁,…,t_K①在t_k时刻，更新粒子权值并且归一化②重采样；利用有效抽样尺度N_eff来衡量粒子权值的退化程度设定一个有效样本数N_threshold作为阈值，若N_eff＜N_threshold，则进行重采样，得到新的粒子群所有粒子权值置为1/N；③状态估计目标节点在t_k时刻的状态信息为得到目标节点在t_k时刻的理论坐标值④更新状态信息将步骤六中得到的t_k时刻目标节点的最终定位坐标值(x_tk,y_tk)赋值给步骤(6)具体如下：设置第二阈值δ₂，利用不等式：判断步骤(4)、(5)得到的两次估计结果和是否相近，若满足不等式，则利用残差加权公式(15)得到t_k时刻目标节点的最终定位坐标值(x_tk,y_tk)，利用该坐标值分别更新扩展卡尔曼滤波TDOA算法和粒子滤波TDOA算法中目标节点前一时刻的状态信息；若不满足不等式，则结束此次定位过程，返回步骤(1)读取TOA原始数据；其中，对应的残差平方和为对应的残差平方和为