[发明专利]在非视距环境下基于距离和角度概率模型的EKF定位方法

摘要

本发明提供了一种在非视距环境下基于距离和角度概率模型的EKF定位方法，用于对移动目标(x,y)进行定位，具有这样的特征，包括以下步骤：步骤1，在多个基站和样本点间找出存在非视距因素的样本点；步骤2，求出距离区间中存在非视距因素的样本点的概率；步骤3，求出角度区间中存在非视距因素的样本点的概率；步骤4，进行多项式曲线拟合，得到距离‑非视距概率和角度‑非视距概率；步骤5，求得第i个基站(xi,yi)与移动目标(x,y)间存在非视距因素的概率；步骤6，求取到每个基站的概率密度函数；步骤7，对移动目标(x,y)的位置进行估计；步骤8，根据估计位置来校正目标与基站的距离；步骤9，根据卡尔曼滤波算法对移动目标的状态进行预测、校正以及更新来完成定位。

主权项

1.一种在非视距环境下基于距离和角度概率模型的EKF定位方法，用于对移动目标(x,y)进行定位，其特征在于，包括以下步骤：步骤1，设置n个所述基站和多个样本点，将第i个所述基站(xi,yi)与第j个所述样本点之间的实际距离记为dr,i,j，方向角记为θr,i,j，测量距离记为zi,j，所述测量距离zi,j为进行多次测量后取的均值，设定一个阀值与所述实际距离dr,i,j和所述测量距离zi,j的差值进行比较，找出存在非视距因素的所述样本点；步骤2，将第i个所述基站(x_i,y_i)与所述多个样本点之间的最大距离记为d_rmax,i，将距离区间(0,d_rmax,i]平均划分为n_di个距离小区间，其中第i_ds个所述距离小区间的距离范围为每个所述距离小区间对应的中心点为将第i_ds个所述距离小区间中的所述样本点的个数记为同时将其中存在所述非视距因素的所述样本点的个数记为求得在第i_ds个所述距离小区间中存在所述非视距因素的概率步骤3，将所述方向角θ_r,i,j的角度区间(‑π,π]平均划分为n_θi个角度小区间，其中第i_θs个所述角度小区间的范围为第i_θs个所述角度小区间对应的中心点为将第i_θs个所述角度小区间中的所述样本点的个数记为并将其中存在所述非视距因素的所述样本点的个数记为求得在第i_θs个所述角度小区间中存在所述非视距因素的概率步骤4，根据每个所述小距离区间的所述中心点d_c,i、每个所述角度小区间的所述中心点θ_c,i、对应的每个所述小距离区间中存在所述非视距因素的概率以及每个所述角度小区间中存在所述非视距因素的概率进行多项式曲线拟合，得到距离‑非视距概率和角度‑非视距概率步骤5，根据所述距离‑非视距概率以及所述角度‑非视距概率得到第i个所述基站(xi,yi)与所述移动目标(x,y)间存在所述非视距因素的概率pi,nlos＝αi·pdi,nlos+(1‑αi)·pθi,nlos；步骤6，根据每个所述基站在视距环境下的概率密度函数和在非视距环境下的概率密度函数结合第i个所述基站(xi,yi)与所述移动目标(x,y)间存在所述非视距因素的概率后得到每个所述基站的概率密度函数p(zi)＝pnlos(zi)·pi,nlos+plos(zi)·(1‑pi,nlos)，根据每个所述基站的概率密度函数建立联合密度函数步骤7，根据位置确定公式：来确定所述移动目标(x,y)所在位置，计算得到的即为对所述移动目标(x,y)所在位置的最优估计，中包括横坐标的估计位置以及纵坐标的估计位置步骤8，根据求得的所述移动目标(x,y)的横坐标的估计位置以及纵坐标的估计位置来校正所述移动目标(x,y)与第i个所述基站(x_i,y_i)之间的距离，所述基站的数量为n个，距离校正公式如下：步骤9，构建所述移动目标(x,y)的预测模型：然后利用卡尔曼滤波算法对所述移动目标(x,y)的状态变量X进行预测、校正以及更新，所述状态变量X中包括所述移动目标(x,y)的x坐标、y坐标、x方向速度以及y方向速度，预测阶段：Xk|k‑1＝AXk‑1|k‑1，Pk|k‑1＝APk‑1|k‑1AT+Q，上述式中，X_k‑1|k‑1＝[x(k‑1),y(k‑1),v_x(k‑1),v_y(k‑1)]^T，X_k‑1|k‑1为k‑1时刻对所述状态变量X的估计，X_k|k‑1为k时刻对所述状态变量X的预测，X_k|k‑1＝[x_k|k‑1，y_k|k‑1，v_x，k|k‑1，v_y，k|k‑1]^T，P_k|k‑1为k时刻的预测误差协方差矩阵，P_k‑1|k‑1为k‑1时刻的校正误差协方差矩阵，Q为过程噪声协方差矩阵，过程噪声协方差矩阵设置为对角矩阵，校正阶段：上述式中，(xk|k‑1,yk|k‑1)从所述预测阶段预测的所述移动目标(x,y)在k时刻的状态变量Xk|k‑1中获取，h(Xk|k‑1)为所述移动目标(x,y)在k时刻的预测位置(xk|k‑1,yk|k‑1)与所述n个基站之间的距离，Hk为向量h(Xk|k‑1)的雅可比矩阵，更新阶段：Kk＝Pk|k‑1HkT(HkPk|k‑1HkT+R)，Xk|k＝Xk|k‑1+Kk(dk′‑hk)，Pk|k＝Pk|k‑1‑KkHkPk|k‑1，上述式中，R为测量协方差矩阵，该测量协方差矩阵设置为对角矩阵，求得的Kk为卡尔曼增益，dk′为所述步骤8中求得的校正距离，hk为k时刻的h(Xk|k‑1)，所述更新阶段求得的状态向量Xk|k即为最终所需要的状态向量，所述状态向量中包括所述移动目标(x,y)在k时刻的x坐标、y坐标、x方向速度以及y方向速度，其中，所述步骤4中的aai和abi为多项式曲线拟合后的系数，pdi,nlos和pθi,nlos分别为在距离dai和角度θbi下存在所述非视距因素的概率，所述步骤5中的k₀为正的常数，θ_i＝atan2(y_i‑y,x_i‑x)，所述步骤9中的x(k)与vx(k)为k时刻所述移动目标(x,y)在世界坐标系下X轴方向的位置与速度，y(k)与vy(k)为k时刻所述移动目标(x,y)在世界坐标系下Y轴方向的位置与速度。