[發(fā)明專利]基于地圖信息和位置自適應(yīng)修正的粒子濾波室內(nèi)定位方法

有效

• 申請?zhí)枺?/td>: 201611007033.4
• 申請日: 2016-11-16
• 公開（公告）號: CN106643724B
• 公開（公告）日: 2019-07-05
• 發(fā)明（設(shè)計）人: 宦若虹;陳月;陶一凡;楊鵬
• 申請（專利權(quán)）人: 浙江工業(yè)大學(xué)
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/10
• 代理公司: 杭州斯可睿專利事務(wù)所有限公司 33241
• 代理人: 王利強

1.一種基于地圖信息和位置自適應(yīng)修正的粒子濾波室內(nèi)定位方法，其特征在于：所述定位方法包括以下步驟：

步驟1，粒子初始化為指定粒子攜帶的狀態(tài)信息為其中是初始時刻t₀第i個粒子的狀態(tài)向量，分別是時刻t₀第i個粒子的橫坐標、縱坐標和權(quán)重，初始權(quán)重設(shè)為1，N是粒子數(shù)量，是由目標初始位置加上偏移量得到，偏移量服從均值為0，方差為σ²的高斯分布；

步驟2，粒子傳播，根據(jù)式(1)由k-1時刻粒子狀態(tài)向量S_k-1計算出粒子在k時刻的狀態(tài)向量S_k，其中是k時刻第i個粒子的坐標，是k-1時刻第i個粒子的坐標，L_k-1是k-1時刻目標的行進步長，L_k是k時刻目標的行進步長，由式(2)計算得到，其中α，β是步長模型參數(shù)，是在離線階段根據(jù)已知步長和步頻通過線性回歸計算得到，freq_k是k時刻的目標步頻，根據(jù)目標行進過程中采集到的加速度傳感器數(shù)據(jù)通過波峰檢測算法計算得到，是k時刻第i個粒子的航向，是k-1時刻第i個粒子的航向，由方向傳感器采集得到；

L_k＝α·freq_k+β (2)

步驟3，粒子更新，判斷粒子在傳播過程中是否與地圖中的墻壁障礙物相交，如果存在相交則該粒子滅亡，權(quán)重置為0；

步驟4，目標位置確定，根據(jù)式(3)計算得到目標在k時刻的最終位置，其中(X_k，Y_k)是k時刻由粒子濾波算法估算出的目標最終位置坐標，是k時刻第i個粒子的坐標，是k時刻第i個粒子的權(quán)重；

步驟5，粒子重采樣和位置自適應(yīng)修正，設(shè)步驟3中得到的存活粒子數(shù)為N_s，在存活的粒子中隨機抽取N-N_s個粒子用于補償損失的粒子；在粒子補償時對補償?shù)牧Ｗ游恢酶鶕?jù)式(4)進行調(diào)整，其中是在k時刻隨機抽取的存活粒子的位置坐標，是在k時刻補償粒子的位置坐標，是k時刻的航向偏差修正參數(shù)，分別是的x軸分量和y軸分量，由式(5)計算得到，是k時刻重采樣粒子i的位置偏移量，服從均值為0，方差為σ²的高斯分布，分別是的x軸分量和y軸分量；是k時刻存活粒子位置距離目標位置的平均偏移量，分別是的x軸分量和y軸分量，是k-1時刻重采樣后粒子位置距離目標位置的平均偏移量，分別是的x軸分量和y軸分量；μ_k是k時刻的位置修正增益參數(shù)，由式(10)計算得到，其中表示