一種機(jī)器人自主構(gòu)建室內(nèi)地圖的路徑規(guī)劃法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及機(jī)器人技術(shù)領(lǐng)域，尤其涉及一種機(jī)器人自主構(gòu)建室內(nèi)地圖的路徑規(guī)劃 算法。
【背景技術(shù)】
[0002] 目前移動(dòng)機(jī)器人常用的地圖描述方法有四種：柵格地圖、拓?fù)涞貓D、直接表征法和 幾何特征地圖，由于機(jī)器人對(duì)所測(cè)得的障礙物具體位置不太敏感，需研發(fā)一種具有直觀簡(jiǎn) 潔、分辨率可變、容易創(chuàng)建和存儲(chǔ)等特點(diǎn)，適用于室內(nèi)環(huán)境路徑規(guī)劃地圖模型的建立的路徑 規(guī)劃法。

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

[0003] 本發(fā)明的目的在于針對(duì)現(xiàn)有技術(shù)的不足而提供一種機(jī)器人自主構(gòu)建室內(nèi)地圖的 路徑規(guī)劃法，該路徑規(guī)劃法直觀簡(jiǎn)潔、分辨率可變、容易創(chuàng)建和存儲(chǔ)。
[0004] 為達(dá)到上述目的，本發(fā)明通過以下技術(shù)方案來(lái)實(shí)現(xiàn)。
[0005] -種機(jī)器人自主構(gòu)建室內(nèi)地圖的路徑規(guī)劃法，包括如下步驟： 步驟1 :初始化； 步驟2 :采用激光雷達(dá)采集數(shù)據(jù)和SLAM構(gòu)建地圖； 步驟3 :采用靜態(tài)牛耕模式?jīng)Q策下個(gè)準(zhǔn)構(gòu)圖節(jié)點(diǎn)； 步驟4 :決策出下個(gè)準(zhǔn)構(gòu)圖節(jié)點(diǎn)，則進(jìn)入局部動(dòng)態(tài)優(yōu)先級(jí)模式確定下個(gè)構(gòu)圖節(jié)點(diǎn)； 步驟5 :決策出下個(gè)構(gòu)圖節(jié)點(diǎn)，則利用A*算法規(guī)劃出到下個(gè)構(gòu)圖節(jié)點(diǎn)的路徑； 步驟6 :根據(jù)規(guī)劃出的路徑，機(jī)器人運(yùn)動(dòng)到下一個(gè)構(gòu)圖點(diǎn)； 步驟7 :運(yùn)用"死胡同"逃離策略在構(gòu)圖路徑中查找逃離節(jié)點(diǎn)； 步驟8 :當(dāng)查找到逃離目標(biāo)后，則利用A*算法規(guī)劃逃離路徑； 步驟9 :構(gòu)圖完成，路徑規(guī)劃結(jié)束。
[0006] 其中，所述靜態(tài)牛耕模式的公式為
[0007] 其中，所述A*算法的公式為.
其中:逾>)是指起始節(jié)點(diǎn)S運(yùn)動(dòng)到節(jié) 點(diǎn)的消耗值；術(shù)！是指節(jié)點(diǎn)運(yùn)動(dòng)到目標(biāo)節(jié)點(diǎn)G估計(jì)的消耗值；是指從起點(diǎn)節(jié)點(diǎn)經(jīng)過節(jié) 點(diǎn)到達(dá)目標(biāo)節(jié)點(diǎn)的最小代價(jià)路徑的估計(jì)值。
[0008] 本發(fā)明的有益效果為：本發(fā)明所述的一種機(jī)器人自主構(gòu)建室內(nèi)地圖的路徑規(guī)劃 法，包括如下步驟：步驟1 :初始化；步驟2 :采用激光雷達(dá)采集數(shù)據(jù)和SLAM構(gòu)建地圖；步驟 3 :采用靜態(tài)牛耕模式?jīng)Q策下個(gè)準(zhǔn)構(gòu)圖節(jié)點(diǎn)；步驟4 :決策出下個(gè)準(zhǔn)構(gòu)圖節(jié)點(diǎn)，則進(jìn)入局部動(dòng) 態(tài)優(yōu)先級(jí)模式確定下個(gè)構(gòu)圖節(jié)點(diǎn)；步驟5 :決策出下個(gè)構(gòu)圖節(jié)點(diǎn)，則利用A*算法規(guī)劃出到下 個(gè)構(gòu)圖節(jié)點(diǎn)的路徑；步驟6 :根據(jù)規(guī)劃出的路徑，機(jī)器人運(yùn)動(dòng)到下一個(gè)構(gòu)圖點(diǎn)；步驟7 :運(yùn)用 "死胡同"逃離策略在構(gòu)圖路徑中查找逃離節(jié)點(diǎn)；步驟8 :當(dāng)查找到逃離目標(biāo)后，則利用A*算 法規(guī)劃逃離路徑；步驟9 :構(gòu)圖完成，路徑規(guī)劃結(jié)束，本發(fā)明具有直觀簡(jiǎn)潔、分辨率可變、容 易創(chuàng)建和存儲(chǔ)的優(yōu)點(diǎn)。
【附圖說明】
[0009] 下面利用附圖來(lái)對(duì)本發(fā)明進(jìn)行進(jìn)一步的說明，但是附圖中的實(shí)施例不構(gòu)成對(duì)本發(fā) 明的任何限制。
[0010] 圖1為本發(fā)明的路徑規(guī)劃算法流程。
【具體實(shí)施方式】
[0011] 下面結(jié)合具體的實(shí)施方式來(lái)對(duì)本發(fā)明進(jìn)行說明。
[0012] 如圖1所示，一種機(jī)器人自主構(gòu)建室內(nèi)地圖的路徑規(guī)劃法，包括如下步驟： 步驟1 :初始化； 步驟2 :采用激光雷達(dá)采集數(shù)據(jù)和SLAM構(gòu)建地圖； 步驟3 :采用靜態(tài)牛耕模式?jīng)Q策下個(gè)準(zhǔn)構(gòu)圖節(jié)點(diǎn)； 步驟4 :決策出下個(gè)準(zhǔn)構(gòu)圖節(jié)點(diǎn)，則進(jìn)入局部動(dòng)態(tài)優(yōu)先級(jí)模式確定下個(gè)構(gòu)圖節(jié)點(diǎn)； 步驟5 :決策出下個(gè)構(gòu)圖節(jié)點(diǎn)，則利用A*算法規(guī)劃出到下個(gè)構(gòu)圖節(jié)點(diǎn)的路徑； 步驟6 :根據(jù)規(guī)劃出的路徑，機(jī)器人運(yùn)動(dòng)到下一個(gè)構(gòu)圖點(diǎn)； 步驟7 :運(yùn)用"死胡同"逃離策略在構(gòu)圖路徑中查找逃離節(jié)點(diǎn)； 步驟8 :當(dāng)查找到逃離目標(biāo)后，則利用A*算法規(guī)劃逃離路徑； 步驟9 :構(gòu)圖完成，路徑規(guī)劃結(jié)束。
[0013] 進(jìn)一步的，所述靜態(tài)牛耕模式的公式為
[0014] 進(jìn)一步的，所述A*算法的公式為
其中是指起始節(jié)點(diǎn)S運(yùn)動(dòng) 到節(jié)點(diǎn)的消耗值；是指節(jié)點(diǎn)運(yùn)動(dòng)到目標(biāo)節(jié)點(diǎn)G估計(jì)的消耗值；及砂是指從起點(diǎn)節(jié)點(diǎn)經(jīng) 過節(jié)點(diǎn)到達(dá)目標(biāo)節(jié)點(diǎn)的最小代價(jià)路徑的估計(jì)值。
[0015] 需更進(jìn)一步的解釋，本發(fā)明采用柵格法描述，機(jī)器人有八個(gè)前進(jìn)方向，假定走正 方位的距離為則走斜方位的距離為6 HK/5,取近似值為14,則代價(jià)函數(shù)的表示如下：
式中，賽/_表示節(jié)點(diǎn)的父節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)S的消 耗值；成表示節(jié)點(diǎn)到其父節(jié)點(diǎn)消耗值；*(略的估計(jì)方法有很多，在柵格地圖常用的方法為 曼哈頓距離估計(jì)、歐氏距離估計(jì)，本文使用的是曼哈頓距離估計(jì)，忽略了對(duì)角距離，具體表 示如下：
其中：:釋代表節(jié)點(diǎn)的坐標(biāo)，;霧￥:和代表目標(biāo)節(jié)點(diǎn)G 的坐標(biāo)；"死胡同"逃離策略：機(jī)器人在構(gòu)圖路徑規(guī)劃時(shí)，會(huì)以堆棧的數(shù)據(jù)結(jié)構(gòu)存儲(chǔ)構(gòu)圖節(jié) 點(diǎn)路徑，當(dāng)機(jī)器人進(jìn)入"死胡同"時(shí)，會(huì)以"先進(jìn)后出"的策略逐個(gè)搜索構(gòu)圖節(jié)點(diǎn)滾動(dòng)窗口以 外柵格，查找出可逃離的節(jié)點(diǎn)。
[0016] 以上內(nèi)容僅為本發(fā)明的較佳實(shí)施例，對(duì)于本領(lǐng)域的普通技術(shù)人員，依據(jù)本發(fā)明的 思想，在【具體實(shí)施方式】及應(yīng)用范圍上均會(huì)有改變之處，本說明書內(nèi)容不應(yīng)理解為對(duì)本發(fā)明 的限制。
【主權(quán)項(xiàng)】
1. 一種機(jī)器人自主構(gòu)建室內(nèi)地圖的路徑規(guī)劃法，其特征在于，包括如下步驟： 步驟1 :初始化； 步驟2 :采用激光雷達(dá)采集數(shù)據(jù)和SLAM構(gòu)建地圖； 步驟3 :采用靜態(tài)牛耕模式?jīng)Q策下個(gè)準(zhǔn)構(gòu)圖節(jié)點(diǎn)； 步驟4 :決策出下個(gè)準(zhǔn)構(gòu)圖節(jié)點(diǎn)，則進(jìn)入局部動(dòng)態(tài)優(yōu)先級(jí)模式確定下個(gè)構(gòu)圖節(jié)點(diǎn)； 步驟5 :決策出下個(gè)構(gòu)圖節(jié)點(diǎn)，則利用A*算法規(guī)劃出到下個(gè)構(gòu)圖節(jié)點(diǎn)的路徑； 步驟6 :根據(jù)規(guī)劃出的路徑，機(jī)器人運(yùn)動(dòng)到下一個(gè)構(gòu)圖點(diǎn)； 步驟7 :運(yùn)用"死胡同"逃離策略在構(gòu)圖路徑中查找逃離節(jié)點(diǎn)； 步驟8 :當(dāng)查找到逃離目標(biāo)后，則利用A*算法規(guī)劃逃離路徑； 步驟9 :構(gòu)圖完成，路徑規(guī)劃結(jié)束。2. 根據(jù)權(quán)利要求1所述的一種機(jī)器人自主構(gòu)建室內(nèi)地圖的路徑規(guī)劃法，其特征在于： 所述靜態(tài)牛耕模式的公式為3. 根據(jù)權(quán)利要求1所述的一種機(jī)器人自主構(gòu)建室內(nèi)地圖的路徑規(guī)劃法，其特征在于： 所述A*算法的公式為/知）二君知) +Κκ)，其中換:)是指起始節(jié)點(diǎn)S運(yùn)動(dòng)到節(jié)點(diǎn)的消耗 值；滿g是指節(jié)點(diǎn)運(yùn)動(dòng)到目標(biāo)節(jié)點(diǎn)G估計(jì)的消耗值；巧對(duì)是指從起點(diǎn)節(jié)點(diǎn)經(jīng)過節(jié)點(diǎn)到達(dá)目 標(biāo)節(jié)點(diǎn)的最小代價(jià)路徑的估計(jì)值。
【專利摘要】本發(fā)明公開了一種機(jī)器人自主構(gòu)建室內(nèi)地圖的路徑規(guī)劃法，包括如下步驟：步驟1：初始化；步驟2：采用激光雷達(dá)采集數(shù)據(jù)和SLAM構(gòu)建地圖；步驟3：采用靜態(tài)牛耕模式?jīng)Q策下個(gè)準(zhǔn)構(gòu)圖節(jié)點(diǎn)；步驟4：決策出下個(gè)準(zhǔn)構(gòu)圖節(jié)點(diǎn)，則進(jìn)入局部動(dòng)態(tài)優(yōu)先級(jí)模式確定下個(gè)構(gòu)圖節(jié)點(diǎn)；步驟5：決策出下個(gè)構(gòu)圖節(jié)點(diǎn)，則利用A*算法規(guī)劃出到下個(gè)構(gòu)圖節(jié)點(diǎn)的路徑；步驟6：根據(jù)規(guī)劃出的路徑，機(jī)器人運(yùn)動(dòng)到下一個(gè)構(gòu)圖點(diǎn)；步驟7：運(yùn)用“死胡同”逃離策略在構(gòu)圖路徑中查找逃離節(jié)點(diǎn)；步驟8：當(dāng)查找到逃離目標(biāo)后，則利用A*算法規(guī)劃逃離路徑；步驟9：構(gòu)圖完成，路徑規(guī)劃結(jié)束，本發(fā)明具有直觀簡(jiǎn)潔、分辨率可變、容易創(chuàng)建和存儲(chǔ)的優(yōu)點(diǎn)。
【IPC分類】G05D1/02
【公開號(hào)】CN105320134
【申請(qǐng)?zhí)枴緾N201510701063
【發(fā)明人】杜元源, 曾碧, 林偉
【申請(qǐng)人】廣東雷洋智能科技股份有限公司, 廣東工業(yè)大學(xué)
【公開日】2016年2月10日
【申請(qǐng)日】2015年10月26日