[發(fā)明專利]一種三維避障的室內機器人導航方法和系統(tǒng)

有效

• 申請?zhí)枺?/td>: 201710186301.1
• 申請日: 2017-03-24
• 公開（公告）號: CN106949893B
• 公開（公告）日: 2019-05-21
• 發(fā)明（設計）人: 陶文兵;鄭龍玉;熊雄
• 申請（專利權）人: 華中科技大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 華中科技大學專利中心 42201
• 代理人: 李智;曹葆青

1.一種三維避障的室內機器人導航方法，其特征在于，包括：

(1)采集機器人的三維模型、機器人行進的初始位置和終點位置，以及包含初始位置和終點位置的全局三維地圖；

(2)根據初始位置、終點位置和全局三維地圖進行全局路徑規(guī)劃，生成一系列全局路徑點，在每一全局路徑點處進行三維障礙物碰撞檢測，若不會發(fā)生碰撞，則得到最優(yōu)的全局路徑規(guī)劃，若會發(fā)生碰撞，則根據初始位置、終點位置和全局三維地圖重新進行全局路徑規(guī)劃直至避開障礙物，得到最優(yōu)的全局路徑規(guī)劃；

(3)機器人按照最優(yōu)的全局路徑規(guī)劃行進，采集機器人的實時位置，拍攝包含實時位置和下一全局路徑點的局部三維地圖，利用局部三維地圖更新全局三維地圖得到實時三維地圖；

(4)在行進過程中對下一全局路徑點進行三維障礙物碰撞檢測，若會發(fā)生碰撞，則以實時三維地圖為全局三維地圖，以機器人實時位置為初始位置，返回步驟(2)，若不會發(fā)生碰撞，則繼續(xù)行進直至達到終點位置，實現了機器人的自主導航與三維避障；

所述三維障礙物碰撞檢測的具體實現方式為：

(2-1)機器人在每一全局路徑點處的三維模型表面由N個二維多邊形組成，遍歷機器人三維模型表面的N個多邊形，對N個多邊形使用啟發(fā)式的算法得到N個基元球體；

(2-2)使用啟發(fā)式的算法得到一個能將N個基元球體包圍的大球體；

(2-3)使用寬度優(yōu)先算法構建一個模型二叉樹結構，使用大球體作為模型二叉樹結構的根節(jié)點，父節(jié)點表示一個能將n個基元球體包圍的父球體，子節(jié)點表示一個能將n/2個基元球體包圍的子球體，最底層節(jié)點表示一個能將1個基元球體包圍的最底層球體，n≤N；

(2-4)全局三維地圖表面由N個全局二維多邊形組成，遍歷全局三維地圖表面的N個全局多邊形，對N個全局多邊形使用啟發(fā)式的算法得到N個全局基元球體；使用啟發(fā)式的算法得到一個能將N個全局基元球體包圍的全局大球體；

(2-5)使用寬度優(yōu)先算法構建一個全局二叉樹結構，使用全局大球體作為全局二叉樹結構的全局根節(jié)點，全局父節(jié)點表示一個能將n個全局基元球體包圍的全局父球體，全局子節(jié)點表示一個能將n/2個全局基元球體包圍的全局子球體，全局最底層節(jié)點表示一個能將1個全局基元球體包圍的全局最底層球體，n≤N；

(2-6)設置模型二叉樹結構和全局二叉樹結構之間的距離為無窮大，從模型二叉樹結構和全局二叉樹結構中分別選取節(jié)點；

(2-7)計算選取節(jié)點對應的球體之間的球心距離，若該距離大于等于模型二叉樹結構和全局二叉樹結構之間的距離，則三維模型和全局三維地圖沒有相交，機器人不會發(fā)生碰撞，流程結束，否則檢查兩個節(jié)點是否都是最底層節(jié)點，若是，則進入步驟(2-8)，若不是，則進入步驟(2-10)；

(2-8)檢查選定兩個節(jié)點代表的球體所包覆的多邊形是否在三維空間相交，若是，則確認機器人和全局三維地圖將發(fā)生碰撞，否則設置兩個節(jié)點代表的球體所包覆的多邊形之間的距離為機器人模型二叉樹結構和全局二叉樹結構之間的距離；

(2-9)從機器人模型二叉樹結構和全局二叉樹結構中未檢索過的節(jié)點中各取一個節(jié)點并選定，返回步驟(2-7)；

(2-10)比較兩個節(jié)點在模型二叉樹結構和全局二叉樹結構中的層級，若相同，則隨機選取一個節(jié)點，否則選取層級較高的節(jié)點，選取節(jié)點的兩個子節(jié)點并比較兩個子節(jié)點與另一選定節(jié)點代表的球體之間的距離，選取距離較近的子節(jié)點代替其父節(jié)點，返回步驟(2-7)。

2.一種三維避障的室內機器人導航系統(tǒng)，其特征在于，包括：

第一模塊，用于采集機器人的三維模型、機器人行進的初始位置和終點位置，以及包含初始位置和終點位置的全局三維地圖；

第二模塊，用于根據初始位置、終點位置和全局三維地圖進行全局路徑規(guī)劃，生成一系列全局路徑點，在每一全局路徑點處進行三維障礙物碰撞檢測，若不會發(fā)生碰撞，則得到最優(yōu)的全局路徑規(guī)劃，若會發(fā)生碰撞，則根據初始位置、終點位置和全局三維地圖重新進行全局路徑規(guī)劃直至避開障礙物，得到最優(yōu)的全局路徑規(guī)劃；

第三模塊，用于使機器人按照最優(yōu)的全局路徑規(guī)劃行進，采集機器人的實時位置，拍攝包含實時位置和下一全局路徑點的局部三維地圖，利用局部三維地圖更新全局三維地圖得到實時三維地圖；

第四模塊，用于在行進過程中對下一全局路徑點進行三維障礙物碰撞檢測，若會發(fā)生碰撞，則以實時三維地圖為全局三維地圖，以機器人實時位置為初始位置，執(zhí)行第二模塊，若不會發(fā)生碰撞，則繼續(xù)行進直至達到終點位置，實現了機器人的自主導航與三維避障；

所述三維障礙物碰撞檢測的具體實現方式為：

(2-1)機器人在每一全局路徑點處的三維模型表面由N個二維多邊形組成，遍歷機器人三維模型表面的N個多邊形，對N個多邊形使用啟發(fā)式的算法得到N個基元球體；

(2-2)使用啟發(fā)式的算法得到一個能將N個基元球體包圍的大球體；

(2-3)使用寬度優(yōu)先算法構建一個模型二叉樹結構，使用大球體作為模型二叉樹結構的根節(jié)點，父節(jié)點表示一個能將n個基元球體包圍的父球體，子節(jié)點表示一個能將n/2個基元球體包圍的子球體，最底層節(jié)點表示一個能將1個基元球體包圍的最底層球體，n≤N；

(2-4)全局三維地圖表面由N個全局二維多邊形組成，遍歷全局三維地圖表面的N個全局多邊形，對N個全局多邊形使用啟發(fā)式的算法得到N 個全局基元球體；使用啟發(fā)式的算法得到一個能將N個全局基元球體包圍的全局大球體；

(2-5)使用寬度優(yōu)先算法構建一個全局二叉樹結構，使用全局大球體作為全局二叉樹結構的全局根節(jié)點，全局父節(jié)點表示一個能將n個全局基元球體包圍的全局父球體，全局子節(jié)點表示一個能將n/2個全局基元球體包圍的全局子球體，全局最底層節(jié)點表示一個能將1個全局基元球體包圍的全局最底層球體，n≤N；

(2-6)設置模型二叉樹結構和全局二叉樹結構之間的距離為無窮大，從模型二叉樹結構和全局二叉樹結構中分別選取節(jié)點；

(2-7)計算選取節(jié)點對應的球體之間的球心距離，若該距離大于等于模型二叉樹結構和全局二叉樹結構之間的距離，則三維模型和全局三維地圖沒有相交，機器人不會發(fā)生碰撞，流程結束，否則檢查兩個節(jié)點是否都是最底層節(jié)點，若是，則進入步驟(2-8)，若不是，則進入步驟(2-10)；

(2-8)檢查選定兩個節(jié)點代表的球體所包覆的多邊形是否在三維空間相交，若是，則確認機器人和全局三維地圖將發(fā)生碰撞，否則設置兩個節(jié)點代表的球體所包覆的多邊形之間的距離為機器人模型二叉樹結構和全局二叉樹結構之間的距離；

(2-9)從機器人模型二叉樹結構和全局二叉樹結構中未檢索過的節(jié)點中各取一個節(jié)點并選定，返回步驟(2-7)；

(2-10)比較兩個節(jié)點在模型二叉樹結構和全局二叉樹結構中的層級，若相同，則隨機選取一個節(jié)點，否則選取層級較高的節(jié)點，選取節(jié)點的兩個子節(jié)點并比較兩個子節(jié)點與另一選定節(jié)點代表的球體之間的距離，選取距離較近的子節(jié)點代替其父節(jié)點，返回步驟(2-7)。