[發(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
• 代理公司: 沈陽東大知識(shí)產(chǎn)權(quán)代理有限公司 21109
• 代理人: 李在川

1.一種未知空間自主探索規(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樹；

步驟3：以機(jī)器人所在位置為起點(diǎn)，向Voronoi-RRT樹的所有邊界點(diǎn)使用RRT算法找到一組可行路徑；

步驟4：計(jì)算每條可行路徑的探索增益，選出探索增益最大的一條作為最優(yōu)路徑；

步驟5：采用路徑規(guī)劃算法執(zhí)行動(dòng)態(tài)避障，生成無障礙路徑；

步驟6：采用DWA算法對(duì)無障礙路徑進(jìn)行跟蹤，得出速度指令下發(fā)到機(jī)器人控制平臺(tái)；

步驟7：判斷地下未知空間內(nèi)的最優(yōu)路徑是否探索結(jié)束，即機(jī)器人是否到達(dá)死胡同；若是則調(diào)用全局規(guī)劃器繼續(xù)判斷出對(duì)分支探索效益最高的葉節(jié)點(diǎn)，基于步驟2的采樣點(diǎn)使用Dijkstra算法搜索到此結(jié)點(diǎn)的最短路徑，繼續(xù)對(duì)分支進(jìn)行探索，轉(zhuǎn)置執(zhí)行步驟1，進(jìn)入下一個(gè)循環(huán)；若不是則轉(zhuǎn)置執(zhí)行步驟8；

步驟8：判斷探索是否完成或者電量是否不足以繼續(xù)探索，滿足任意條件則觸發(fā)回家條件，回溯返回起點(diǎn)的最短路徑；

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

所述步驟4的過程如下：

步驟4.1：計(jì)算每條可行路徑σ_i上每個(gè)采樣點(diǎn)的體積增益Gv；

步驟4.2：為了避免機(jī)器人頻繁進(jìn)入各個(gè)分叉路口，引入軌跡點(diǎn)在時(shí)間維度上相似性權(quán)重s(σ_i,σ_e)用于度量當(dāng)前可行路徑σ_i和新探索出的路徑σ_e的軌跡相似性，然后把當(dāng)前可行路徑σ_i上所有采樣點(diǎn)的體積增益做加和，計(jì)算每條可行路徑的探索增益GE(σ_i)，公式如下：

其中，是根節(jié)點(diǎn)延路徑到每個(gè)采樣點(diǎn)的累計(jì)歐式距離，即為用于衡量路徑距離的一個(gè)權(quán)重函數(shù)，參數(shù)p、q均為權(quán)重系數(shù)，m_i為一條路徑上的第m個(gè)軌跡點(diǎn)，s(σ_i,σ_e)為Dynamic?Time?Warping方法計(jì)算軌跡點(diǎn)在時(shí)間維度上相似性權(quán)重；

步驟4.3：以探索增益最大的一條路徑作為最優(yōu)路徑。

2.根據(jù)權(quán)利要求1所述的未知空間自主探索規(guī)劃方法，其特征在于，所述以當(dāng)前環(huán)境中的障礙物為離散點(diǎn)生成維諾圖，并采樣生成Voronoi-RRT樹的過程如下：

S1：在體素格式的地圖中將當(dāng)前環(huán)境邊界離散化處理，以離散化的邊界和離散的障礙物點(diǎn)為頂點(diǎn)構(gòu)建Delaunary三角網(wǎng)，并對(duì)所有離散的點(diǎn)和構(gòu)建好的三角網(wǎng)內(nèi)的每個(gè)三角形編號(hào)；

S2：計(jì)算每個(gè)三角形的外接圓圓心，并存儲(chǔ)每個(gè)三角形對(duì)應(yīng)的離散點(diǎn)；

S3：遍歷三角網(wǎng)鏈表，尋找與當(dāng)前三角形相鄰的三個(gè)三角形，如果找到則把尋找到的三角形外心與當(dāng)前三角形外心相連，將連線作為維諾邊存入維諾邊鏈表；如果找不到則求出當(dāng)前三角形中最靠近機(jī)器人的邊的中垂線存入維諾邊鏈表；

S4：以存儲(chǔ)的離散點(diǎn)為中心，在維諾邊鏈表附近進(jìn)行隨機(jī)的采樣，構(gòu)建出Voronoi-RRT樹。

3.根據(jù)權(quán)利要求1所述的未知空間自主探索規(guī)劃方法，其特征在于，所述步驟4.1的過程如下：

步驟4.1.1：每個(gè)采樣點(diǎn)發(fā)射角度均勻離散的固定等長射線模型；

步驟4.1.2：統(tǒng)計(jì)穿過射線的未知占用體素n_unknown、已知占用體素n_occupied和已知無障礙體素n_free的個(gè)數(shù)；

步驟4.1.3：給每種體素設(shè)置一個(gè)權(quán)重值，并將射線模型上的上述三種體素加權(quán)求和作為采樣點(diǎn)的體積增益，公式如下：

其中，g_unknown為未知占用體素的權(quán)重值，g_occupied為已知占用體素的權(quán)重值，g_free為已知無障礙體素的權(quán)重值。