[發(fā)明專利]基于粒子濾波的定位方法、裝置、計(jì)算機(jī)設(shè)備和存儲(chǔ)介質(zhì)

有效

• 申請(qǐng)?zhí)枺?/td>: 202010230278.3
• 申請(qǐng)日: 2020-03-27
• 公開(kāi)（公告）號(hào): CN111352066B
• 公開(kāi)（公告）日: 2022-02-22
• 發(fā)明（設(shè)計(jì)）人: 沈博;薛勝利
• 申請(qǐng)（專利權(quán)）人: 西安震有信通科技有限公司
• 主分類號(hào): G01S5/02
• 分類號(hào): G01S5/02;G01S5/14
• 代理公司: 深圳市君勝知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 44268
• 代理人: 劉文求;王永文

1.一種基于粒子濾波的定位方法，其特征在于，包括如下步驟：

將礦井中各目標(biāo)對(duì)象通過(guò)UWB協(xié)議與最近的兩個(gè)定位分站通訊，以獲取所述目標(biāo)對(duì)象的原始測(cè)量位置；

創(chuàng)建粒子濾波器，對(duì)粒子數(shù)量及初始狀態(tài)參數(shù)進(jìn)行初始化設(shè)置；

基于狀態(tài)轉(zhuǎn)移方程預(yù)測(cè)所述目標(biāo)對(duì)象的下一時(shí)刻位置；

所述粒子濾波器根據(jù)所述原始測(cè)量位置計(jì)算每個(gè)粒子對(duì)所述目標(biāo)對(duì)象的位置影響權(quán)重，獲取的權(quán)值進(jìn)行歸一化處理；

經(jīng)過(guò)反復(fù)迭代處理后，當(dāng)所述粒子濾波器濾波計(jì)算出目標(biāo)對(duì)象的估計(jì)位置滿足預(yù)定義的偏差閾值時(shí)，將該目標(biāo)對(duì)象的估計(jì)位置作為目標(biāo)對(duì)象的真實(shí)位置記錄并輸出；

所述將礦井中各目標(biāo)對(duì)象通過(guò)UWB協(xié)議與最近的兩個(gè)定位分站通訊，以獲取所述目標(biāo)對(duì)象的原始測(cè)量位置具體包括如下步驟：

在礦井巷道中各目標(biāo)對(duì)象上設(shè)置定位標(biāo)簽；

各目標(biāo)對(duì)象上的定位標(biāo)簽通過(guò)UWB協(xié)議與距離其最近的兩個(gè)定位分站-分站1和分站2，分別進(jìn)行通訊連接，獲取所述目標(biāo)對(duì)象到兩個(gè)定位分站的第一距離r1和第二距離r2；

根據(jù)分站1(x₁,y₁)、分站2(x₂,y₂)的坐標(biāo)、第一距離r1、第二距離r2計(jì)算出以分站1為圓心第一距離r1為半徑的圓和以分站2為圓心第二距離r2為半徑的圓與礦井巷道中心線的四個(gè)交點(diǎn)a1(x_a1,y_a1)，a2(x_a2,y_a2),b1(x_b1,y_b1)，b2(x_b2,y_b2)；

計(jì)算所述四個(gè)交點(diǎn)的兩點(diǎn)間距離a1b1、a1b2、a2b1、a2b2，并取間距最小的一組兩點(diǎn)間距離，計(jì)算其中點(diǎn)位置A(x_a,y_a)作為所述目標(biāo)對(duì)象的原始測(cè)量位置。

2.根據(jù)權(quán)利要求1所述的一種基于粒子濾波的定位方法，其特征在于，所述創(chuàng)建粒子濾波器，對(duì)粒子數(shù)量及初始狀態(tài)參數(shù)進(jìn)行初始化設(shè)置包括如下具體步驟：

構(gòu)建一個(gè)遞歸的貝葉斯?fàn)顟B(tài)估計(jì)器；

設(shè)置粒子數(shù)量≤20個(gè)，初始化每個(gè)粒子的權(quán)重相等，為1/N，其中N為粒子數(shù)量；

根據(jù)前一次的測(cè)量位置、礦井巷道走向和寬度信息將粒子的初始位置設(shè)置于所述礦井巷道的內(nèi)部測(cè)量位置附近。

3.根據(jù)權(quán)利要求1所述的一種基于粒子濾波的定位方法，其特征在于，所述基于狀態(tài)轉(zhuǎn)移方程預(yù)測(cè)所述目標(biāo)對(duì)象的下一時(shí)刻位置中，所述狀態(tài)轉(zhuǎn)移是指所述目標(biāo)對(duì)象運(yùn)動(dòng)狀態(tài)隨時(shí)間的更新，通過(guò)模型中的狀態(tài)方程來(lái)描述所述目標(biāo)對(duì)象運(yùn)動(dòng)到下一時(shí)刻的測(cè)量位置，其中模型為勻速運(yùn)動(dòng)模型和勻加速運(yùn)動(dòng)模型。

4.根據(jù)權(quán)利要求2所述的一種基于粒子濾波的定位方法，其特征在于，所述粒子濾波器根據(jù)所述原始測(cè)量位置計(jì)算每個(gè)粒子對(duì)所述目標(biāo)對(duì)象的位置影響權(quán)重，獲取的權(quán)值進(jìn)行歸一化處理具體包括如下步驟：

所述粒子濾波器獲取所述目標(biāo)對(duì)象的原始測(cè)量位置A(x_a,y_a)；

根據(jù)每個(gè)粒子對(duì)所述目標(biāo)對(duì)象的原始測(cè)量位置A(x_a,y_a)影響的權(quán)重，采用馬爾可夫過(guò)程計(jì)算出每個(gè)粒子對(duì)所述原始測(cè)量位置A(x_a,y_a)影響的權(quán)值

對(duì)所述權(quán)值根據(jù)公式進(jìn)行權(quán)值歸一化處理。