[發(fā)明專利]一種未知空間自主探索規(guī)劃方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202110953148.7
• 申請(qǐng)日: 2021-08-19
• 公開（公告）號(hào): CN113625721B
• 公開（公告）日: 2023-05-16
• 發(fā)明（設(shè)計(jì)）人: 方正;張磊濤;楊寧;龐成林;張遠(yuǎn);李先鋒
• 申請(qǐng)（專利權(quán)）人: 東北大學(xué)
• 主分類號(hào): G05D1/02
• 分類號(hào): G05D1/02
• 代理公司: 沈陽(yáng)東大知識(shí)產(chǎn)權(quán)代理有限公司 21109
• 代理人: 李在川

本發(fā)明提供一種未知空間自主探索規(guī)劃方法，方法獲取機(jī)器人當(dāng)前的定位信息生成三維點(diǎn)云地圖，以機(jī)器人為中心生成維諾圖，基于維諾圖路徑中心采樣生成Voronoi?RRT；以機(jī)器人所在位置為起點(diǎn)向Voronoi?RRT樹的所有邊界點(diǎn)使用RRT算法找到可行路徑，計(jì)算每條路徑的探索增益，以探索增益最大為最優(yōu)路徑；采用路徑規(guī)劃算法執(zhí)行動(dòng)態(tài)避障生成無(wú)碰撞候選路徑，當(dāng)路徑被動(dòng)態(tài)障礙物遮擋的時(shí)候規(guī)劃到其他候選路徑上繼續(xù)運(yùn)動(dòng)；采用DWA動(dòng)態(tài)窗口算法對(duì)最終的路徑跟蹤，得出速度指令下發(fā)到機(jī)器人控制平臺(tái)。當(dāng)機(jī)器人進(jìn)入死胡同，或者機(jī)器人電量不足時(shí)調(diào)用全局規(guī)劃器，牽引到之前標(biāo)記的探索空間邊界或返回出發(fā)點(diǎn)。實(shí)現(xiàn)快速、安全、高效的地下未知空間自主探索。

技術(shù)領(lǐng)域

本發(fā)明涉及路徑規(guī)劃技術(shù)領(lǐng)域，尤其涉及一種未知空間自主探索規(guī)劃方法。

背景技術(shù)

近年來，機(jī)器人技術(shù)飛速發(fā)展，自主移動(dòng)機(jī)器人在探索人類不適宜居住、工作的環(huán)境如礦井，隧道等方面的研究越來越多。由于地下未知環(huán)境的復(fù)雜性，比如復(fù)雜深邃的地形，通信不發(fā)達(dá)導(dǎo)致不能直接對(duì)機(jī)器人進(jìn)行遠(yuǎn)程的控制，以及不能利用GPS進(jìn)行導(dǎo)航等。在這種復(fù)雜環(huán)境下，自主移動(dòng)機(jī)器人需要是高度自主化、魯棒、可靠和適應(yīng)在復(fù)雜環(huán)境下運(yùn)動(dòng)的。自主探索技術(shù)指導(dǎo)機(jī)器人在未知環(huán)境下自主運(yùn)動(dòng)，依靠各種傳感器感知未知環(huán)境，完成搜索、救援、探測(cè)等具體任務(wù)。盡管眾多研究者在硬件系統(tǒng)、軟件算法上都取得了重大進(jìn)展，但復(fù)雜的地下未知環(huán)境仍然對(duì)機(jī)器人的自主探索能力造成巨大的挑戰(zhàn)。尤其是對(duì)地下未知多分支環(huán)境的探索，這種環(huán)境的特性是通常規(guī)模很大，分支路徑長(zhǎng)，狹窄分支多。此外，它們的地形通常非常粗糙，碎石等障礙物較多，對(duì)機(jī)器人的避障性能要求非常高。

目前，移動(dòng)機(jī)器人自主探索中使用的主流方法之一是基于邊界的探索方法。機(jī)器人獲取到傳感器感知范圍內(nèi)的環(huán)境信息，在感知到的已知環(huán)境內(nèi)生成快速搜索隨機(jī)樹，用聚類算法對(duì)快速搜索隨機(jī)樹的邊界點(diǎn)進(jìn)行聚類得到質(zhì)心點(diǎn)；構(gòu)建得到的質(zhì)心點(diǎn)收益函數(shù)，計(jì)算每個(gè)質(zhì)心點(diǎn)的收益值；選擇收益值最大的質(zhì)心點(diǎn)作為目標(biāo)點(diǎn)，使用A*全局路徑規(guī)劃算法在已知的環(huán)境中快速規(guī)劃出一條由機(jī)器人當(dāng)前位置到目標(biāo)點(diǎn)的路徑，引導(dǎo)機(jī)器人向目標(biāo)點(diǎn)移動(dòng)；重復(fù)以上步驟直至沒有侯選的質(zhì)心點(diǎn)，結(jié)束探索。

然而這種算法效率和避障效果并不高。它需要在局部地圖內(nèi)隨機(jī)撒點(diǎn)，由于地下未知環(huán)境中障礙物較多，很多采樣點(diǎn)撒在障礙物上是無(wú)效的；在狹窄分支下，有效的采樣點(diǎn)主要集中在狹窄分支的路徑上，這也造成了大量采樣點(diǎn)無(wú)效，對(duì)這些采樣點(diǎn)進(jìn)行搜索計(jì)算浪費(fèi)了大量時(shí)間。并且由于隨機(jī)撒點(diǎn)導(dǎo)致某些目標(biāo)點(diǎn)距離障礙物或者井道較近，對(duì)機(jī)器人造成了潛在的碰撞風(fēng)險(xiǎn)。此外，由于地下未知空間多分支的特性，目前的探索方法應(yīng)用于這種環(huán)境會(huì)隨機(jī)進(jìn)入各個(gè)分支，探索效率較低。

另外，一種基于簡(jiǎn)化廣義Voronoi圖的機(jī)器人自主探索方法也得到了應(yīng)用，這種基于形態(tài)學(xué)方法構(gòu)建簡(jiǎn)化廣Voronoi拓?fù)涞貓D，找到最佳前沿點(diǎn)，并規(guī)劃?rùn)C(jī)器人當(dāng)前位置到最佳前沿點(diǎn)的全局路徑，沿全局路徑將機(jī)器人導(dǎo)航至最佳前沿點(diǎn)。

然而，單純基于Voronoi圖生成拓?fù)涞貓D，在拓?fù)涞貓D里找到前沿點(diǎn)，再利用Dijkstra算法搜索到最佳前沿點(diǎn)的最短路徑，這種僅基于原始Voronoi圖找前沿點(diǎn)的方法應(yīng)用于隧道環(huán)境會(huì)忽略某些探索增益大的狹小分支，造成探索不完整，并且由于地下未知空間多分支的特性，這種探索方法應(yīng)用于多分支環(huán)境會(huì)隨機(jī)進(jìn)入各個(gè)分支，探索效率較低。

發(fā)明內(nèi)容

針對(duì)上述現(xiàn)有技術(shù)的不足，本發(fā)明提供一種未知空間自主探索規(guī)劃方法。

為解決上述技術(shù)問題，本發(fā)明所采取的技術(shù)方案是：一種未知空間自主探索規(guī)劃方法，包括如下步驟：

步驟1：通過三維激光傳感器和IMU采用三維SLAM算法獲取機(jī)器人當(dāng)前的定位信息和感知信息生成當(dāng)前環(huán)境的三維點(diǎn)云地圖；

步驟2：在機(jī)器人周圍設(shè)定一個(gè)滑動(dòng)窗口，包含已探索區(qū)域和未知區(qū)域，在滑動(dòng)窗口內(nèi)以機(jī)器人當(dāng)前位置為起點(diǎn)，以當(dāng)前環(huán)境中的障礙物為離散點(diǎn)生成維諾圖，并采樣生成Voronoi-RRT樹；

所述維諾圖使用體素格式。