[發明專利]在非視距環境下基于距離和角度概率模型的EKF定位方法

有效

• 申請號: 201810992786.8
• 申請日: 2018-08-29
• 公開（公告）號: CN109141427B
• 公開（公告）日: 2022-01-25
• 發明（設計）人: 田昕;魏國亮;王永雄;管啟
• 申請（專利權）人: 上海理工大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 上海德昭知識產權代理有限公司 31204
• 代理人: 郁旦蓉

1.一種在非視距環境下基于距離和角度概率模型的EKF定位方法，用于對移動目標(x,y)進行定位，其特征在于，包括以下步驟：

步驟1，設置n個基站和多個樣本點，將第i個所述基站(x_i,y_i)與第j個所述樣本點之間的實際距離記為d_r,i,j，方向角記為θ_r,i,j，測量距離記為z_i,j，所述測量距離z_i,j為進行多次測量后取的均值，設定一個閾值與所述實際距離d_r,i,j和所述測量距離z_i,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個所述基站(x_i,y_i)與所述移動目標(x,y)間存在所述非視距因素的概率p_i,nlos＝α_i·p_di,nlos+(1-α_i)·p_θi,nlos；

步驟6，根據每個所述基站在視距環境下的概率密度函數

和在非視距環境下的概率密度函數

結合第i個所述基站(x_i,y_i)與所述移動目標(x,y)間存在所述非視距因素的概率后得到每個所述基站的概率密度函數

p(z_i)＝p_nlos(z_i)·p_i,nlos+p_los(z_i)·(1-p_i,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方向速度，

預測階段：X_k|k-1＝AX_k-1|k-1，

P_k|k-1＝AP_k-1|k-1A^T+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為過程噪聲協方差矩陣，過程噪聲協方差矩陣設置為對角矩陣，

校正階段：

上述式中，(x_k|k-1,y_k|k-1)從所述預測階段預測的所述移動目標(x,y)在k時刻的狀態變量X_k|k-1中獲取，h(X_k|k-1)為所述移動目標(x,y)在k時刻的預測位置(x_k|k-1,y_k|k-1)與所述n個基站之間的距離，H_k為向量h(X_k|k-1)的雅可比矩陣，

更新階段：

K_k＝P_k|k-1H_k^T(H_kP_k|k-1H_k^T+R)，

X_k|k＝X_k|k-1+K_k(d_k′-h_k)，

P_k|k＝P_k|k-1-K_kH_kP_k|k-1，

上述式中，R為測量協方差矩陣，該測量協方差矩陣設置為對角矩陣，求得的Kk為卡爾曼增益，d_k′為所述步驟8中求得的校正距離，h_k為k時刻的h(X_k|k-1)，

所述更新階段求得的狀態向量X_k|k即為最終所需要的狀態向量，所述狀態向量中包括所述移動目標(x,y)在k時刻的x坐標、y坐標、x方向速度以及y方向速度，

其中，所述步驟4中的a_ai和a_bi為多項式曲線擬合后的系數，p_di,nlos和p_θi,nlos分別為在距離d_ai和角度θ_bi下存在所述非視距因素的概率，

所述步驟5中的k₀為正的常數，θ_i＝atan2(y_i-y,x_i-x)，

所述步驟9中的x(k)與v_x(k)為k時刻所述移動目標(x,y)在世界坐標系下X軸方向的位置與速度，y(k)與v_y(k)為k時刻所述移動目標(x,y)在世界坐標系下Y軸方向的位置與速度。