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

有效

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

本發明提供了一種在非視距環境下基于距離和角度概率模型的EKF定位方法，用于對移動目標(x,y)進行定位，具有這樣的特征，包括以下步驟：步驟1，在多個基站和樣本點間找出存在非視距因素的樣本點；步驟2，求出距離區間中存在非視距因素的樣本點的概率；步驟3，求出角度區間中存在非視距因素的樣本點的概率；步驟4，進行多項式曲線擬合，得到距離?非視距概率和角度?非視距概率；步驟5，求得第i個基站(x_i,y_i)與移動目標(x,y)間存在非視距因素的概率；步驟6，求取到每個基站的概率密度函數；步驟7，對移動目標(x,y)的位置進行估計；步驟8，根據估計位置來校正目標與基站的距離；步驟9，根據卡爾曼濾波算法對移動目標的狀態進行預測、校正以及更新來完成定位。

技術領域

本發明涉及一種定位方法，具體涉及一種在非視距環境下基于距離和角度概率模型的EKF定位方法。

背景技術

移動機器人代表了機電一體化較高的水平，該機器人的廣泛應用以自主導航作為前提，而較高的導航精度離不開較高的定位精度。在室內環境中布置傳感器網絡可以較好完成機器人定位與導航的功能。

現有技術中，通常使用多個距離信息組成多個距離方程，使用最小二乘法求出目標點的坐標，從而使得方程的殘差最小，但是由于室內環境的復雜性，傳感器網絡在測距過程中由于遮擋等原因，所得的距離信息會與實際距離信息產生較大差異，從而會使得定位信息存在較大偏差，如何根據傳感器網絡的距離信息來識別并削弱由于非視距產生的誤差，從而提高在非視距環境中的定位精度也成為了亟待解決的問題，因此提出了本發明的一種在非視距環境下基于距離和角度概率模型的EKF定位方法。

發明內容

本發明是為了解決上述問題而進行的，目的在于提供一種在非視距環境下基于距離和角度概率模型的EKF定位方法。

本發明提供了一種在非視距環境下基于距離和角度概率模型的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、對應的每個小距離區間中存在非視距因素的概率以及每個角度小區間中存在非視距因素的概率進行多項式曲線擬合，得到距離-非視距概率和角度-非視距概率