[發(fā)明專利]基于粒子濾波算法的巡檢機(jī)器人動態(tài)路徑規(guī)劃方法及裝置

有效

• 申請?zhí)枺?/td>: 202110574036.0
• 申請日: 2021-05-25
• 公開（公告）號: CN113359714B
• 公開（公告）日: 2022-07-08
• 發(fā)明（設(shè)計(jì)）人: 姚楠;單光瑞;潘建亞;朱睿;劉子全;尹康涌;王真;張昱;秦劍華;楠萌萌;朱雪瓊;黃浩聲;王靜君;梁偉
• 申請（專利權(quán)）人: 國網(wǎng)江蘇省電力有限公司電力科學(xué)研究院;國網(wǎng)江蘇省電力有限公司;江蘇省電力試驗(yàn)研究院有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 蘇州市中南偉業(yè)知識產(chǎn)權(quán)代理事務(wù)所(普通合伙) 32257
• 代理人: 丁博寒

1.一種基于粒子濾波算法的巡檢機(jī)器人動態(tài)路徑規(guī)劃方法，其特征在于，包括：

S101：對巡檢機(jī)器人任務(wù)場景創(chuàng)建地圖，建立路徑滾動窗口；

S102：利用傳感器獲取當(dāng)前時(shí)刻的環(huán)境信息，所述環(huán)境信息包括所述任務(wù)場景中障礙物和所述巡檢機(jī)器人的位置與移動速度；

S103：根據(jù)所述當(dāng)前時(shí)刻的環(huán)境信息，采用粒子濾波算法預(yù)測所述任務(wù)場景中動態(tài)障礙物和所述巡檢機(jī)器人下一時(shí)刻的位置和移動速度，得到更新后的環(huán)境信息；

S104：根據(jù)所述巡檢機(jī)器人的當(dāng)前位置及任務(wù)終點(diǎn)，確定所述巡檢機(jī)器人在所述路徑滾動窗口內(nèi)的局部路徑起止點(diǎn)；

S105：在所述路徑滾動窗口內(nèi)建立局部坐標(biāo)系，根據(jù)所述更新后的環(huán)境信息和所述局部路徑起止點(diǎn)，在所述局部坐標(biāo)系中生成所述巡檢機(jī)器人的初始路徑樣本集；

S106：在所述路徑滾動窗口內(nèi)迭代執(zhí)行粒子濾波算法估計(jì)路徑樣本，直至迭代次數(shù)達(dá)到預(yù)設(shè)次數(shù)，輸出目標(biāo)局部路徑，以便所述巡檢機(jī)器人根據(jù)所述目標(biāo)局部路徑前進(jìn)一步；

S107：判斷所述巡檢機(jī)器人是否到達(dá)所述任務(wù)終點(diǎn)，若否，則將所述路徑滾動窗口向前推進(jìn)一步，循環(huán)執(zhí)行S102-S107直至所述巡檢機(jī)器人到達(dá)所述任務(wù)終點(diǎn)；

所述根據(jù)所述巡檢機(jī)器人的當(dāng)前位置及任務(wù)終點(diǎn)，確定所述巡檢機(jī)器人在所述路徑滾動窗口內(nèi)的局部路徑起止點(diǎn)包括：

根據(jù)所述巡檢機(jī)器人的當(dāng)前位置與任務(wù)終點(diǎn)，確定所述路徑巡檢窗口內(nèi)的基準(zhǔn)點(diǎn)；

根據(jù)所述基準(zhǔn)點(diǎn)，確定所述巡檢機(jī)器人的局部任務(wù)點(diǎn)，從而確定所述巡檢機(jī)器人在所述路徑滾動窗口內(nèi)的局部路徑起止點(diǎn)；

所述根據(jù)所述巡檢機(jī)器人的當(dāng)前位置與任務(wù)終點(diǎn)，確定所述路徑巡檢窗口內(nèi)的基準(zhǔn)點(diǎn)包括：

確定所述巡檢機(jī)器人的當(dāng)前位置與所述任務(wù)終點(diǎn)的連線，選取所述連線與所述路徑滾動窗口的交匯點(diǎn)作為所述基準(zhǔn)點(diǎn)；

所述根據(jù)所述基準(zhǔn)點(diǎn)，確定所述巡檢機(jī)器人的局部任務(wù)點(diǎn)包括：

以所述基準(zhǔn)點(diǎn)為中心，在預(yù)設(shè)生成范圍內(nèi)選取所述巡檢機(jī)器人的局部任務(wù)點(diǎn)；

其中，所述預(yù)設(shè)生成范圍服從標(biāo)準(zhǔn)差為的高斯分布，L₀為全局路徑長度，L_k為當(dāng)前時(shí)刻的剩余待規(guī)劃路徑長度，δ_max為所述預(yù)設(shè)生成范圍的最大標(biāo)準(zhǔn)差。

2.如權(quán)利要求1所述的方法，其特征在于，所述在所述路徑滾動窗口內(nèi)建立局部坐標(biāo)系包括：

以所述巡檢機(jī)器人的當(dāng)前位置與所述基準(zhǔn)點(diǎn)的連線作為坐標(biāo)系橫軸，在所述路徑滾動窗口內(nèi)建立局部坐標(biāo)系，以便采用一組N維向量表示一條路徑樣本X＝[x₀，x₁，…，x_D]，其中，D＝L/s為一次滾動規(guī)劃的步數(shù)，L為所述路徑滾動窗口的大小，s為規(guī)劃步長，N＝D+1，x₀為所述巡檢機(jī)器人的當(dāng)前位置，x₁為所述巡檢機(jī)器人的局部路徑點(diǎn)，x_D為所述巡檢機(jī)器人的局部路徑終點(diǎn)。

3.如權(quán)利要求2所述的方法，其特征在于，所述根據(jù)所述更新后的環(huán)境信息和所述局部路徑起止點(diǎn)，在所述局部坐標(biāo)系中生成所述巡檢機(jī)器人的初始路徑樣本集包括：

根據(jù)所述更新后的環(huán)境信息和所述局部路徑起止點(diǎn)，采用正交化算法在所述局部坐標(biāo)系中生成所述巡檢機(jī)器人的初始路徑樣本集。

4.如權(quán)利要求1所述的方法，其特征在于，所述在所述路徑滾動窗口內(nèi)迭代執(zhí)行粒子濾波算法估計(jì)路徑樣本，直至迭代次數(shù)達(dá)到預(yù)設(shè)次數(shù)，輸出目標(biāo)局部路徑，以便所述巡檢機(jī)器人根據(jù)所述目標(biāo)局部路徑前進(jìn)一步包括：

S701：將所述初始路徑樣本集輸入至粒子濾波算法估計(jì)路徑樣本，并記錄所述粒子濾波算法的當(dāng)前迭代次數(shù)；

S702：根據(jù)路徑評價(jià)函數(shù)計(jì)算每個路徑樣本的權(quán)值，并根據(jù)所述每個路徑樣本的權(quán)值估計(jì)當(dāng)前最優(yōu)路徑樣本；

S703：判斷所述當(dāng)前迭代次數(shù)是否達(dá)到預(yù)設(shè)次數(shù)；

S704：若所述當(dāng)前迭代次數(shù)未達(dá)到所述預(yù)設(shè)次數(shù)，則在所述路徑滾動窗口內(nèi)對所述巡檢機(jī)器人的局部路徑點(diǎn)進(jìn)行重采樣，得到更新后的路徑樣本集與當(dāng)前迭代次數(shù)，返回執(zhí)行S702；

S705：若所述當(dāng)前迭代次數(shù)達(dá)到所述預(yù)設(shè)次數(shù)，則輸出所述當(dāng)前最優(yōu)路徑樣本作為目標(biāo)局部路徑，以便所述巡檢機(jī)器人根據(jù)所述目標(biāo)局部路徑前進(jìn)一步。