一種提高機(jī)器人路徑規(guī)劃效率的室內(nèi)地圖構(gòu)建方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及室內(nèi)機(jī)器人領(lǐng)域，尤其涉及一種提高機(jī)器人路徑規(guī)劃效率的室內(nèi)地圖 構(gòu)建方法。
【背景技術(shù)】
[0002] 近來年，隨著科技的發(fā)展和進(jìn)步，在機(jī)器人領(lǐng)域，室內(nèi)服務(wù)機(jī)器人掀起了一股新的 研宄和應(yīng)用熱潮。
[0003] 機(jī)器人在室內(nèi)環(huán)境中的自主定位和導(dǎo)航需要活動(dòng)空間的環(huán)境地圖作為前提，精確 地地圖表示和創(chuàng)建成為機(jī)器人自主移動(dòng)的關(guān)鍵技術(shù)，同時(shí)也是完成其它室內(nèi)任務(wù)的基礎(chǔ)。 常用的環(huán)境表示方法有柵格地圖，幾何表示地圖，特征地圖和拓?fù)涞貓D。
[0004] 其中，柵格地圖即將整個(gè)環(huán)境分為若干個(gè)大小相同的柵格。在柵格地圖中，通過某 種規(guī)劃算法，找到一條代價(jià)最小，連結(jié)起點(diǎn)（機(jī)器人當(dāng)前柵格位置）和終點(diǎn)（目標(biāo)點(diǎn)所在的 柵格位置）的路徑。在實(shí)際應(yīng)用中，柵格地圖的精度和大小往往相互矛盾，精度越高，柵格 的數(shù)目會(huì)相應(yīng)增多，導(dǎo)致路徑規(guī)劃算法的規(guī)劃空間過大，使得規(guī)劃時(shí)間急劇增加，甚至無法 響應(yīng)實(shí)時(shí)需要。
[0005]拓?fù)涞貓D相比柵格地圖具有高度的抽象性，適用于環(huán)境大而簡(jiǎn)單的情況。但是，該 方法缺乏環(huán)境的具體信息，難以適應(yīng)定位等需要環(huán)境特征的任務(wù)。

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

[0006] 本發(fā)明的目的是提供一種提高機(jī)器人路徑規(guī)劃效率的室內(nèi)地圖構(gòu)建方法，使得機(jī) 器人導(dǎo)航中的路徑規(guī)劃算法更加簡(jiǎn)單和高效。
[0007] 本發(fā)明的目的是通過以下技術(shù)方案實(shí)現(xiàn)的：
[0008]-種提高機(jī)器人路徑規(guī)劃效率的室內(nèi)地圖構(gòu)建方法，該方法包括：
[0009]步驟A、將待掃描的室內(nèi)環(huán)境的拓?fù)涞貓D表示為包含房間集合V與門集合E的無向 圖G= <V，E>;
[0010] 步驟B、初始化房間集合V= {SJ，門集合￡ = 0，并標(biāo)定機(jī)器人當(dāng)前所在的房間節(jié) 點(diǎn)利用機(jī)器人內(nèi)置的激光傳感器在當(dāng)前位置對(duì)當(dāng)前房間進(jìn)行掃描；其中，將獲 得柵格數(shù)據(jù)的標(biāo)簽值Label設(shè)為\，并從中提取出邊界柵格，獲得邊界點(diǎn)集合；
[0011] 步驟C、對(duì)所述邊界點(diǎn)集合進(jìn)行預(yù)處理，獲得候選探索點(diǎn)集合，并基于效用函數(shù)從 候選探索點(diǎn)集合中選擇下一步探索的目標(biāo)點(diǎn)；
[0012] 步驟D、該機(jī)器人向所述目標(biāo)點(diǎn)移動(dòng)，利用獲得的柵格數(shù)據(jù)對(duì)初始柵格數(shù)據(jù)進(jìn)行更 新，并判斷是否檢測(cè)到房門；若未檢測(cè)到門，將獲得的柵格數(shù)據(jù)的標(biāo)簽值Label設(shè)為&若 檢測(cè)到門，則將門同側(cè)的柵格數(shù)據(jù)的標(biāo)簽值Label設(shè)為\，將門另一側(cè)的柵格數(shù)據(jù)的標(biāo)簽值 Label設(shè)為Si，并更新房間集合V= {SQ，SJ，門集合E=EU{e<SQ，Sp};
[0013]步驟E、提取所述目標(biāo)點(diǎn)位置處采集到的柵格數(shù)據(jù)的邊界點(diǎn)集合，并執(zhí)行上述步驟C~步驟D，直至完成當(dāng)前室內(nèi)環(huán)境的掃描過程。
[0014] 進(jìn)一步的，該方法還包括：當(dāng)檢測(cè)到門，并設(shè)置了門兩側(cè)柵格數(shù)據(jù)的標(biāo)簽值Label 后，判斷是否出現(xiàn)標(biāo)簽值Label被重復(fù)設(shè)置的情況；
[0015] 若是，且設(shè)置前的標(biāo)簽值Labels與設(shè)置后的標(biāo)簽值Labelnew不相同，則將相應(yīng)柵 格數(shù)據(jù)的標(biāo)簽值為重新設(shè)置為設(shè)置前的標(biāo)簽值Labels，并將房間集合V中標(biāo)簽值Labels與Labelnew對(duì)應(yīng)的房間節(jié)點(diǎn)合并。
[0016] 進(jìn)一步的，所述對(duì)所述邊界點(diǎn)集合進(jìn)行預(yù)處理，獲得候選探索點(diǎn)集合包括：
[0017] 根據(jù)點(diǎn)之間的距離將邊界點(diǎn)分成若干子區(qū)域，并剔除子區(qū)域內(nèi)的離異點(diǎn)；
[0018] 利用Hough直線檢測(cè)算法，用直線分別擬合每一子區(qū)域中的邊界點(diǎn)，提取邊界線 段，再對(duì)線段進(jìn)行延伸與合并處理，獲得若干線段；
[0019] 記錄各線段的端點(diǎn)和角度，取線段的中點(diǎn)沿中垂線朝外的方向?yàn)楹蜻x探索點(diǎn)。
[0020] 進(jìn)一步的，所述基于效用函數(shù)從候選探索點(diǎn)集合中選擇下一步探索的目標(biāo)點(diǎn)包 括：
[0021] 基于效用函數(shù)計(jì)算每一候選探索點(diǎn)的函數(shù)值，其公式為：
[0023] 其中，S(P)表示在候選探索點(diǎn)P處激光覆蓋范圍內(nèi)未知區(qū)域的面積，C(P)表示在 候選探索點(diǎn)P處激光覆蓋范圍內(nèi)局部柵格地圖和障礙物地圖上角點(diǎn)的數(shù)目，A0 (P)為候 選探索點(diǎn)P的朝向和當(dāng)前機(jī)器人的朝向之間的夾角，D(P)為機(jī)器人當(dāng)前位置與候選探索點(diǎn) P的距離，a取值范圍為[0, 1]。
[0024] 將函數(shù)值最大的候選點(diǎn)作為下一步探索的目標(biāo)點(diǎn)。
[0025] 進(jìn)一步的，該方法還包括：根據(jù)激光數(shù)據(jù)檢測(cè)室內(nèi)環(huán)境中的門，其步驟為：
[0026]由于障礙物的存在，激光數(shù)據(jù)被劃分為若干連續(xù)的區(qū)間，通過下述公式計(jì)算障礙 物的寬度：
[0027]Width=L: ?L2 ?cos(b-a)
[0028] 其中，1^與L2*別為機(jī)器人在當(dāng)前位置處與障礙物兩側(cè)的距離，a與b分別為機(jī) 器人當(dāng)前位置處的水平方向與障礙物兩側(cè)的夾角；
[0029] 若滿足下式，則判定該障礙物為門：
[0031] 其中，fz是待掃描的室內(nèi)環(huán)境中門寬度的平均值，DIST_T0L表示可以容忍的寬度 差別。
[0032] 由上述本發(fā)明提供的技術(shù)方案可以看出，將拓?fù)涞貓D與柵格地圖相結(jié)合，使得規(guī) 劃問題發(fā)生在拓?fù)涔?jié)點(diǎn)表示的空間內(nèi)，避免了全局空間的規(guī)劃；同時(shí)，利用激光識(shí)別室內(nèi)環(huán) 境中的門，并建立起房間之間的拓?fù)潢P(guān)系，再將該過程融入到粒子濾波SLAM(同時(shí)定位與 建圖）算法中，最終得到環(huán)境表示的柵格地圖和基于柵格地圖的拓?fù)涞貓D，使得機(jī)器人導(dǎo) 航中的路徑規(guī)劃算法更加簡(jiǎn)單和高效。
【附圖說明】
[0033] 為了更清楚地說明本發(fā)明實(shí)施例的技術(shù)方案，下面將對(duì)實(shí)施例描述中所需要使用 的附圖作簡(jiǎn)單地介紹，顯而易見地，下面描述中的附圖僅僅是本發(fā)明的一些實(shí)施例，對(duì)于本 領(lǐng)域的普通技術(shù)人員來講，在不付出創(chuàng)造性勞動(dòng)的前提下，還可以根據(jù)這些附圖獲得其他 附圖。
[0034]圖1為本發(fā)明實(shí)施例提供的一種提高機(jī)器人路徑規(guī)劃效率的室內(nèi)地圖構(gòu)建方法；
[0035]圖2為本發(fā)明實(shí)施例提供的激光數(shù)據(jù)被分割為連續(xù)區(qū)域的示意圖；
[0036] 圖3為本發(fā)明實(shí)施例提供的激光數(shù)據(jù)的直方圖；
[0037]圖4為本發(fā)明實(shí)施例提供的初始探索狀態(tài)的示意圖；
[0038] 圖5為本發(fā)明實(shí)施例提供的機(jī)器人檢測(cè)到門時(shí)的示意圖；
[0039] 圖6為本發(fā)明實(shí)施例提供的機(jī)器人檢測(cè)到門時(shí)的示意圖；
[0040]圖7為本發(fā)明實(shí)施例提供的處理柵格數(shù)據(jù)標(biāo)簽值重復(fù)設(shè)置時(shí)的示意圖；
[0041]圖8為本發(fā)明實(shí)施例提供的處理柵格數(shù)據(jù)標(biāo)簽值重復(fù)設(shè)置時(shí)的示意圖；
[0042] 圖9為本發(fā)明實(shí)施例提供的實(shí)驗(yàn)時(shí)所構(gòu)建實(shí)際環(huán)境的示意圖；
[0043] 圖10為本發(fā)明實(shí)施例提供的基于本發(fā)明方案進(jìn)行實(shí)驗(yàn)的結(jié)果示意圖。
【具體實(shí)施方式】
[0044] 下面結(jié)合本發(fā)明實(shí)施例中的附圖，對(duì)本發(fā)明實(shí)施例中的技術(shù)方案進(jìn)行清楚、完整 地描述，顯然，所描述的實(shí)施例僅僅是本發(fā)明一部分實(shí)施例，而不是全部的實(shí)施例?；诒?發(fā)明的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒有做出創(chuàng)造性勞動(dòng)前提下所獲得的所有其他實(shí)施 例，都屬于本發(fā)明的保護(hù)范圍。
[0045] 實(shí)施例一
[0046] 圖1為本發(fā)明實(shí)施例提供的一種提高機(jī)器人路徑規(guī)劃效率的室內(nèi)地圖構(gòu)建方法。 如圖1所示，該方法主要包括如下步驟：
[0047] 步驟11、將待掃描的室內(nèi)環(huán)境的拓?fù)涞貓D表示為包含房間集合V與門集合E的無 向圖 G=<V，E>。
[0048]本發(fā)明實(shí)施例所建立的拓?fù)鋱D以房間為節(jié)點(diǎn)，房間之間的門作為邊，本方案中基 于門的檢測(cè)和機(jī)器人的即時(shí)移動(dòng)建立拓?fù)涞貓D，而且同時(shí)創(chuàng)建柵格地圖。
[0049] 室內(nèi)環(huán)境中，通過門的位置就可以將區(qū)分不同的房間，而現(xiàn)有的房間分割方法都 是通過提取出已有的柵格地圖上的線段，角點(diǎn)，多邊形等幾何特征，計(jì)算復(fù)雜且需要提前建 立柵格地圖。
[0050]步驟12、初始化房間集合V= {S。}，門集合五二0，并標(biāo)定機(jī)器人當(dāng)前所在的房間 節(jié)點(diǎn)利用機(jī)器人內(nèi)置的激光傳感器在當(dāng)前位置對(duì)當(dāng)前房間進(jìn)行掃描；其中，將 獲得柵格數(shù)據(jù)的標(biāo)簽值Label設(shè)為\，并從中提取出邊界柵格，獲得邊界點(diǎn)集合。
[0051] 步驟13、對(duì)所述邊界點(diǎn)集合進(jìn)行預(yù)處理，獲得候選探索點(diǎn)集合，并基于效用函數(shù)從 候選探索點(diǎn)集合中選擇下一步探索的目標(biāo)點(diǎn)。
[0052] 步驟14、該機(jī)器人向所述目標(biāo)點(diǎn)移動(dòng)，利用獲得的柵格數(shù)據(jù)對(duì)初始柵格數(shù)據(jù)進(jìn)行 更新，并判斷是否檢測(cè)到房門；若未檢測(cè)到門，將獲得的柵格數(shù)據(jù)的標(biāo)簽值Label設(shè)為 若檢測(cè)到門，則將門同側(cè)的柵格數(shù)據(jù)的標(biāo)簽值Label設(shè)為\，將門另一側(cè)的柵格數(shù)據(jù)的標(biāo)簽 值Label設(shè)為Si，并更新房間集合V = {'SJ，門集合E = E U {eCS^Si〉}。
[0053] 更新后的門集合E = E U {e < Sd, Si> }表示新增一個(gè)用于連接房間節(jié)點(diǎn)S 與 Si的邊元素e<SQ,Si>。
[0054] 進(jìn)一步的，當(dāng)檢測(cè)到門，并設(shè)置了門兩側(cè)柵格數(shù)據(jù)的標(biāo)簽值Label后，判斷是否出 現(xiàn)標(biāo)簽值Label被重復(fù)設(shè)置的情況；
[0055] 若是，且設(shè)置前的標(biāo)簽值Labels與設(shè)置后的標(biāo)簽值Label new不相同，則將相應(yīng)柵 格數(shù)據(jù)的標(biāo)簽值為重新設(shè)置為設(shè)置前的標(biāo)簽值Labels，并將房間集合V中標(biāo)簽值Labels 與Label new對(duì)應(yīng)的房間節(jié)點(diǎn)合并。
[0056] 步驟15、提取所述目標(biāo)點(diǎn)位置處采集到的柵格數(shù)據(jù)的邊界點(diǎn)集合，并執(zhí)行上述步 驟13~步驟14,直至完成當(dāng)前室內(nèi)環(huán)境的掃描過程。
[0057] 為了便于理解本發(fā)明，下面分別針對(duì)門檢測(cè)、探索策略及建立房間拓?fù)潢P(guān)系做詳 細(xì)的說明。
[0058] 1、利用激光數(shù)據(jù)實(shí)現(xiàn)室內(nèi)環(huán)境中的門檢測(cè)。
[0059] 機(jī)器人在建圖的過程中，不斷接受周圍環(huán)境的激光數(shù)據(jù)，對(duì)激光數(shù)據(jù)經(jīng)行分析就 可以檢測(cè)出環(huán)境中的門。
[0060] 如圖2所示，由于障礙物的存在，激光數(shù)據(jù)被劃分成為幾個(gè)連續(xù)的區(qū)間，圖2中的 門（door)即為一障礙物，a與b分別為機(jī)器人當(dāng)前位置處的水平方向（X軸）與門兩側(cè)的 夾角；為了更為直觀的表示圖2,可以將其轉(zhuǎn)換為圖3所示的直方圖，圖3左側(cè)的space則 為門之間的寬度。
[0061]計(jì)算障礙物寬度的公式為：
[0062] Width = L: ? L2 ? cos(b-a)
[0063] 其中，1^與L2分別為機(jī)器人在當(dāng)前位置處與障礙物兩側(cè)的距離；
[0064] 若滿足下式，則判定該障礙物為門：
[0066]其中，汞是待掃描的室內(nèi)環(huán)境中門寬度的平均值，DIST_T0L表示可以容忍的寬度 差別。
[0067]當(dāng)檢測(cè)到門以后，則可根據(jù)機(jī)器人的位置計(jì)算得到門的兩個(gè)端點(diǎn)的坐標(biāo)，從而獲 得門的位置和朝向。
[0068] 2、探索策略。
[0069] 機(jī)器人在探索的過程中不斷的趨向于未知區(qū)域，獲取未知區(qū)域的信息來擴(kuò)充地 圖，為了保證機(jī)器人探索的效率和地圖的準(zhǔn)確性，探索活動(dòng)需要遵循一定的策略和方法。 [0