[發(fā)明專利]一種自適應(yīng)膨脹處理方法、系統(tǒng)、機(jī)器人及存儲(chǔ)介質(zhì)

有效

• 申請(qǐng)?zhí)枺?/td>: 202010929771.4
• 申請(qǐng)日: 2020-09-07
• 公開(kāi)（公告）號(hào): CN112066976B
• 公開(kāi)（公告）日: 2023-06-16
• 發(fā)明（設(shè)計(jì)）人: 谷玉海;龔志力;左云波;王少紅;徐小力
• 申請(qǐng)（專利權(quán)）人: 北京信息科技大學(xué)
• 主分類號(hào): G01C21/00
• 分類號(hào): G01C21/00;G01C21/32
• 代理公司: 北京遠(yuǎn)創(chuàng)理想知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 11513
• 代理人: 張素妍

1.一種自適應(yīng)膨脹處理方法，該方法基于代價(jià)地圖實(shí)現(xiàn)，其特征在于，包括以下步驟：

1)獲取機(jī)器人所處環(huán)境的柵格地圖，對(duì)障礙點(diǎn)坐標(biāo)進(jìn)行初始標(biāo)記；

2)獲得障礙點(diǎn)的坐標(biāo)后，填充機(jī)器人傳感器位置到障礙點(diǎn)之間的柵格以及該障礙點(diǎn)柵格的初始代價(jià)值；

3)對(duì)障礙點(diǎn)進(jìn)行自適應(yīng)膨脹處理，根據(jù)柵格代價(jià)值填充障礙點(diǎn)周?chē)蛎泤^(qū)柵格的代價(jià)值，完成對(duì)代價(jià)地圖的膨脹處理；

所述步驟3)中，自適應(yīng)膨脹處理包括以下步驟：

3.1)由初始障礙點(diǎn)構(gòu)造膨脹點(diǎn)優(yōu)先隊(duì)列inflation_cells；

3.2)將隊(duì)列inflation_cells的第一個(gè)節(jié)點(diǎn)作為當(dāng)前處理節(jié)點(diǎn)，計(jì)算當(dāng)前處理節(jié)點(diǎn)的父節(jié)點(diǎn)的衰減系數(shù)coef；

3.3)若當(dāng)前處理節(jié)點(diǎn)為障礙點(diǎn)，則以其為中心在柵格地圖上構(gòu)建七宮圖；反之，若當(dāng)前處理節(jié)點(diǎn)不是障礙點(diǎn)，則繼承其當(dāng)前處理障礙點(diǎn)O_n的衰減因子sp，并根據(jù)計(jì)算其衰減系數(shù)coef和衰減因子sp的乘積coef·sp，進(jìn)入步驟3.4)；

3.4)判斷d(q_n)＜coef·sp·r_c是否成立，若成立，則將當(dāng)前處理障礙點(diǎn)O_n的上下左右四個(gè)子節(jié)點(diǎn)加入隊(duì)列inflation_cells，否則不做處理；

3.5)由自適應(yīng)膨脹半徑柵格代價(jià)值計(jì)算函數(shù)f(q_n)計(jì)算出當(dāng)前處理節(jié)點(diǎn)的代價(jià)值，與該處理節(jié)點(diǎn)的原始代價(jià)值比較，取最大值作為當(dāng)前處理節(jié)點(diǎn)的代價(jià)值；

3.6)從隊(duì)列inflation_cells中移除當(dāng)前處理節(jié)點(diǎn)即障礙點(diǎn)，返回步驟3.2)對(duì)下一個(gè)節(jié)點(diǎn)進(jìn)行處理；

所述步驟3.2)中，衰減系數(shù)coef計(jì)算方法為：

3.2.1)判斷該當(dāng)前處理節(jié)點(diǎn)是否為障礙點(diǎn)，若是，則將該當(dāng)前處理節(jié)點(diǎn)自身作為父節(jié)點(diǎn)，此時(shí)的父節(jié)點(diǎn)即為當(dāng)前處理障礙點(diǎn)O_n；若不是障礙點(diǎn)，則記離該當(dāng)前處理節(jié)點(diǎn)最近的障礙點(diǎn)作為父節(jié)點(diǎn)；

3.2.2)確定當(dāng)前節(jié)點(diǎn)與其父節(jié)點(diǎn)O_n之間的距離d(q_n)，以及父節(jié)點(diǎn)O_n與機(jī)器人的距離計(jì)算當(dāng)前處理節(jié)點(diǎn)的父節(jié)點(diǎn)O_n的衰減系數(shù)coef；

當(dāng)前處理障礙點(diǎn)O_n的衰減系數(shù)coef為：

式中，r_i表示機(jī)器人地面投影輪廓的內(nèi)切圓半徑；r_c表示外接圓半徑；q_n為柵格；D_max為距機(jī)器人最遠(yuǎn)障礙點(diǎn)的距離；n表示障礙點(diǎn)的個(gè)數(shù)；e為自然對(duì)數(shù)函數(shù)的底數(shù)；為當(dāng)前處理障礙點(diǎn)O_n與機(jī)器人之間的柵格距離；

所述步驟3.3)中，衰減因子sp的計(jì)算方法為：依次得到七宮圖中的其余48個(gè)柵格代價(jià)值，累計(jì)得出值為FREE_SPACE的柵格數(shù)量n_F和值為L(zhǎng)ETHAL_OBSTACLE的柵格數(shù)量n_L，則衰減因子sp為：

所述步驟3.4)中，對(duì)加入隊(duì)列inflation_cells的子節(jié)點(diǎn)進(jìn)行如下處理：

將四個(gè)子節(jié)點(diǎn)依次作為當(dāng)前處理節(jié)點(diǎn)，判斷其與父節(jié)點(diǎn)的距離是否在膨脹半徑內(nèi)，在內(nèi)則繼續(xù)將該點(diǎn)的子節(jié)點(diǎn)加入膨脹隊(duì)列，此時(shí)記其父節(jié)點(diǎn)仍然為之前的當(dāng)前處理障礙點(diǎn)O_n，在外則不做處理；每處理完一個(gè)節(jié)點(diǎn)后計(jì)算并填充該節(jié)點(diǎn)的代價(jià)值，最后從膨脹隊(duì)列中移除這個(gè)節(jié)點(diǎn)；

所述自適應(yīng)膨脹半徑柵格代價(jià)值計(jì)算函數(shù)f(q_n)為：

式中，r_a表示每個(gè)柵格的邊長(zhǎng)；r_m表示人為規(guī)定的代價(jià)地圖膨脹半徑；w表示代價(jià)值下降權(quán)重；d(q_n)為當(dāng)前節(jié)點(diǎn)與其父節(jié)點(diǎn)之間的距離；r_i表示機(jī)器人地面投影輪廓的內(nèi)切圓半徑；r_c表示外接圓半徑。