070] 2. 1提取未知邊界，獲得邊界點集合。
[0071] 柵格地圖中每個柵格單元的狀態(tài)對應(yīng)于空閑，占用，未知三種狀態(tài)。為了建立完整 的環(huán)境地圖，機器人只有不斷靠近未知區(qū)域才是有意義的。
[0072] 本發(fā)明實施例中，定義柵格狀態(tài)函數(shù)如下：
[0074]上式中，x，y為柵格的坐標(biāo)，唯一確定一個柵格。若一個柵格A為邊界柵格則需要 滿足條件以下兩個條件。
[0075] l)S(xA, yA) = 2 ；
[0076] 2)U為柵格A四鄰域集合，若存在柵格B和C，
且 3C e "a ) = 1;其中，3為存在量詞。
[0077] 通過上述方式找到所有的邊界柵格，則完成了位置邊界的提取，從而獲得邊界點 集合。
[0078] 2)確定候選點。
[0079] 首先，根據(jù)點之間的距離將邊界點分成若干子區(qū)域，并剔除子區(qū)域內(nèi)的離異點；其 次，利用Hough直線檢測算法，用直線分別擬合每一子區(qū)域中的邊界點，提取邊界線段，再 對線段進(jìn)行延伸與合并處理，獲得若干線段；最后，記錄各線段的端點和角度，取線段的中 點沿中垂線朝外的方向為候選探索點。
[0080] 3)確定探索目標(biāo)點。
[0081] 評價一個環(huán)境探索策略的標(biāo)準(zhǔn)是能否高效準(zhǔn)確的建立完整或近似完整的環(huán)境地 圖。本發(fā)明實施例在確定下一步探索點時，綜合考慮了路徑優(yōu)化，信息增益，定位準(zhǔn)確等因 素。
[0082] 未知環(huán)境中的探索，機器人需要根據(jù)已有的局部柵格地圖的信息，確定下一步繼 續(xù)感知環(huán)境的最優(yōu)位置，確保能夠順利完成對未知環(huán)境的探索和建圖。具體的算法步驟如 下：
[0083] 1)若候選點集合為空，則探索結(jié)束；否則轉(zhuǎn)向步驟2)。
[0084] 2)基于效用函數(shù)計算每一候選探索點的函數(shù)值，并將函數(shù)值最大的候選點作為下 一步探索的目標(biāo)點，效用函數(shù)公式為：
[0086] 其中，S(P)表示在候選探索點P處激光覆蓋范圍內(nèi)未知區(qū)域的面積，C(P)表示在 候選探索點P處激光覆蓋范圍內(nèi)局部柵格地圖和障礙物地圖上角點的數(shù)目，A 0 (P)為候 選探索點P的朝向和當(dāng)前機器人的朝向之間的夾角，D(P)為機器人當(dāng)前位置與候選探索 點P的距離，a取值范圍為[0, 1]。
[0087] 3)驅(qū)使機器人到達(dá)目標(biāo)點，并讀取傳感器數(shù)據(jù)，更新局部柵格地圖和障礙物地圖。
[0088] 4)提取局部地圖的邊界點，計算出新的候選點加入候選點列表，轉(zhuǎn)到步驟1)。
[0089] 需要說明的是，在上述探索過程中，機器人還會根據(jù)前述的方式實時判斷是否檢 測到門，以便建立房間的拓?fù)潢P(guān)系，具體的在后文中會進(jìn)行詳細(xì)的描述。
[0090] 3、建立房間拓?fù)潢P(guān)系。
[0091] 通常室內(nèi)環(huán)境房間與房間之間都有一個或多個門，只要能正確檢測出門就能夠得 到單獨的房間和房間之間的拓?fù)潢P(guān)系，而將這種拓?fù)潢P(guān)系應(yīng)用到導(dǎo)航的路徑規(guī)劃算法，可 以使得規(guī)劃更高效而且簡單。
[0092] 本發(fā)明實施例中，將拓?fù)涞貓D表示成為無向圖G = < V，E >，V表示房間節(jié)點集合， E表示門集合；令為機器人的當(dāng)前所在的房間節(jié)點；同時為每一個柵格增加一個標(biāo)簽 值Label，記錄該柵格所在的房間節(jié)點。下面結(jié)合圖4-圖8來進(jìn)行說明，具體的步驟如下：
[0093] 1)探索開始時，V = {S。}，￡ = 0，并設(shè)置Scu"ent= S。，如圖4所示。
[0094] 2)根據(jù)前述方式來確定需探索的目標(biāo)點，并移動至目標(biāo)點，并判斷是否檢測到門， 若未檢測到門轉(zhuǎn)如步驟3)，若檢測到門則轉(zhuǎn)入步驟4)。
[0095] 3)當(dāng)未檢測到門時，將獲得的柵格數(shù)據(jù)的標(biāo)簽值Label值設(shè)為S。。
[0096] 4)當(dāng)未檢測到門時，則將門同側(cè)的柵格數(shù)據(jù)的標(biāo)簽值Label設(shè)為&，將門另一側(cè) 的柵格數(shù)據(jù)的標(biāo)簽值Label設(shè)為Si，并更新房間集合V =隊，SJ，門集合E = E U {e < \，Si > }，即對于無向圖G而言，在節(jié)點\與S澗增加一條邊（在E中新增一個門元素），如圖 5_圖6所示。同時，由于某些房間可能存在多個門，通過上述方式可能出現(xiàn)柵格數(shù)據(jù)的標(biāo)簽 值Label重復(fù)設(shè)置的情況，若出現(xiàn)這一情況，則轉(zhuǎn)入步驟5)，否則，轉(zhuǎn)入步驟6)。
[0097] 5)若設(shè)置前的標(biāo)簽值Label。；^與設(shè)置后的標(biāo)簽值Label new不相同，則將相應(yīng)柵格 數(shù)據(jù)的標(biāo)簽值為重新設(shè)置為設(shè)置前的標(biāo)簽值Labels，并將房間集合V中標(biāo)簽值1^1^1。 1(1與 Labels對應(yīng)的房間節(jié)點合并，并保持邊數(shù)目不變，如圖7-圖8所示。
[0098] 6)更新Sramnt為機器人當(dāng)前所在的房間節(jié)點。
[0099]上述所舉例的圖4-圖8中，左側(cè)為機器人探索的示意圖，右側(cè)為根據(jù)探索結(jié)果構(gòu) 建的無向圖G的示意圖。
[0100] 本發(fā)明實施例通過將拓?fù)涞貓D與柵格地圖相結(jié)合，使得規(guī)劃問題發(fā)生在拓?fù)涔?jié)點 表示的空間內(nèi)，避免了全局空間的規(guī)劃；同時，利用激光識別室內(nèi)環(huán)境中的門，并建立起房 間之間的拓?fù)潢P(guān)系，再將該過程融入到粒子濾波SLAM(同時定位與建圖）算法中，最終得到 環(huán)境表示的柵格地圖和基于柵格地圖的拓?fù)涞貓D，使得機器人導(dǎo)航中的路徑規(guī)劃算法更加 簡單和高效。
[0101] 另一方面，還與本發(fā)明的上述實施例進(jìn)行了實驗。
[0102] 本實驗中，采用了自主研發(fā)的服務(wù)機器人"可佳"，"可佳"的底盤裝有Hokuyo UTM-30LN激光傳感器，該激光傳感器的范圍為0. 1~30m，掃描頻率為60Hz，角度分辨率為 0.25°。"可佳"的計算部件通常為筆記本或工控機（滿足cpu intel i3及以上，內(nèi)存4G 左右）。
[0103] 在應(yīng)用中，操縱機器人在室內(nèi)環(huán)境遍歷一遍就能建立精確地識別出環(huán)境中的門， 并且建立柵格地圖和拓?fù)涞貓D。所建地圖的實際環(huán)境模型如圖9所示，實驗結(jié)果如圖10所 示。從實驗結(jié)果可以看出，在比較復(fù)雜的家居環(huán)境中，利用本發(fā)明的方法能夠比較精確地建 立環(huán)境地圖，本發(fā)明實施例中的探索策略能夠保證環(huán)境地圖的準(zhǔn)確性和完整性，門檢測算 法能夠快速有效的檢測出門，從而建立基于門和房間之間的拓?fù)潢P(guān)系。
[0104] 通過以上的實施方式的描述，本領(lǐng)域的技術(shù)人員可以清楚地了解到上述實施例可 以通過軟件實現(xiàn)，也可以借助軟件加必要的通用硬件平臺的方式來實現(xiàn)?；谶@樣的理解， 上述實施例的技術(shù)方案可以以軟件產(chǎn)品的形式體現(xiàn)出來，該軟件產(chǎn)品可以存儲在一個非易 失性存儲介質(zhì)（可以是CD-ROM，U盤，移動硬盤等）中，包括若干指令用以使得一臺計算機 設(shè)備（可以是個人計算機，服務(wù)器，或者網(wǎng)絡(luò)設(shè)備等）執(zhí)行本發(fā)明各個實施例所述的方法。
[0105] 以上所述，僅為本發(fā)明較佳的【具體實施方式】，但本發(fā)明的保護(hù)范圍并不局限于此， 任何熟悉本技術(shù)領(lǐng)域的技術(shù)人員在本發(fā)明披露的技術(shù)范圍內(nèi)，可輕易想到的變化或替換， 都應(yīng)涵蓋在本發(fā)明的保護(hù)范圍之內(nèi)。因此，本發(fā)明的保護(hù)范圍應(yīng)該以權(quán)利要求書的保護(hù)范 圍為準(zhǔn)。
【主權(quán)項】
1. 一種提高機器人路徑規(guī)劃效率的室內(nèi)地圖構(gòu)建方法，其特征在于，該方法包括： 步驟A、將待掃描的室內(nèi)環(huán)境的拓?fù)涞貓D表示為包含房間集合V與門集合E的無向圖G =< V1E > ； 步驟Β、初始化房間集合V = {SJ，門集合￡ = 0，并標(biāo)定機器人當(dāng)前所在的房間節(jié)點 Scmrart= S ^利用機器人內(nèi)置的激光傳感器在當(dāng)前位置對當(dāng)前房間進(jìn)行掃描；其中，將獲得 柵格數(shù)據(jù)的標(biāo)簽值Label設(shè)為Stl，并從中提取出邊界柵格，獲得邊界點集合； 步驟C、對所述邊界點集合進(jìn)行預(yù)處理，獲得候選探索點集合，并基于效用函數(shù)從候選 探索點集合中選擇下一步探索的目標(biāo)點； 步驟D、該機器人向所述目標(biāo)點移動，利用獲得的柵格數(shù)據(jù)對初始柵格數(shù)據(jù)進(jìn)行更新， 并判斷是否檢測到房門；若未檢測到門，將獲得的柵格數(shù)據(jù)的標(biāo)簽值Label設(shè)為Stl;若檢 測到門，則將門同側(cè)的柵格數(shù)據(jù)的標(biāo)簽值Label設(shè)為Stl，將門另一側(cè)的柵格數(shù)據(jù)的標(biāo)簽值 Label設(shè)為S1，并更新房間集合V = {SQ，SJ，門集合E = E U {e < SQ，S1S }; 步驟E、提取所述目標(biāo)點位置處采集到的柵格數(shù)據(jù)的邊界點集合，并執(zhí)行上述步驟C~ 步驟D，直至完成當(dāng)前室內(nèi)環(huán)境的掃描過程。2. 根據(jù)權(quán)利要求1所述的方法，其特征在于，該方法還包括：當(dāng)檢測到門，并設(shè)置了門 兩側(cè)柵格數(shù)據(jù)的標(biāo)簽值Label后，判斷是否出現(xiàn)標(biāo)簽值Label被重復(fù)設(shè)置的情況； 若是，且設(shè)置前的標(biāo)簽值Labeltjld與設(shè)置后的標(biāo)簽值Label new不相同，則將相應(yīng)柵格數(shù) 據(jù)的標(biāo)簽值為重新設(shè)置為設(shè)置前的標(biāo)簽值Labeltjld，并將房間集合V中標(biāo)簽值1^1^1。1(1與 Labe Inew對應(yīng)的房間節(jié)點合并。3. 根據(jù)權(quán)利要求1所述的方法，其特征在于，所述對所述邊界點集合進(jìn)行預(yù)處理，獲得 候選探索點集合包括： 根據(jù)點之間的距離將邊界點分成若干子區(qū)域，并剔除子區(qū)域內(nèi)的離異點； 利用Hough直線檢測算法，用直線分別擬合每一子區(qū)域中的邊界點，提取邊界線段，再 對線段進(jìn)行延伸與合并處理，獲得若干線段； 記錄各線段的端點和角度，取線段的中點沿中垂線朝外的方向為候選探索點。4. 根據(jù)權(quán)利要求1或3所述的方法，其特征在于，所述基于效用函數(shù)從候選探索點集合 中選擇下一步探索的目標(biāo)點包括： 基于效用函數(shù)計算每一候選探索點的函數(shù)值，其公式為：其中，S(P)表示在候選探索點P處激光覆蓋范圍內(nèi)未知區(qū)域的面積，C(P)表示在候選 探索點P處激光覆蓋范圍內(nèi)局部柵格地圖和障礙物地圖上角點的數(shù)目，Λ Θ (P)為候選探 索點P的朝向和當(dāng)前機器人的朝向之間的夾角，D(P)為機器人當(dāng)前位置與候選探索點P的 距離，α取值范圍為[〇, 1]。 將函數(shù)值最大的候選點作為下一步探索的目標(biāo)點。5. 根據(jù)權(quán)利要求1所述的方法，其特征在于，該方法還包括：根據(jù)激光數(shù)據(jù)檢測室內(nèi)環(huán) 境中的門，其步驟為： 由于障礙物的存在，激光數(shù)據(jù)被劃分為若干連續(xù)的區(qū)間，通過下述公式計算障礙物的 寬度： Width = L1 · L2 · cos(b-a) 其中，1^與L 2分別為機器人在當(dāng)前位置處與障礙物兩側(cè)的距離，a與b分別為機器人 當(dāng)前位置處的水平方向與障礙物兩側(cè)的夾角； 若滿足下式，則判定該障礙物為門：其中，r是待掃描的室內(nèi)環(huán)境中門寬度的平均值，dist_tol表示可以容忍的寬度差別。
【專利摘要】本發(fā)明公開了一種提高機器人路徑規(guī)劃效率的室內(nèi)地圖構(gòu)建方法，該方法將拓?fù)涞貓D與柵格地圖相結(jié)合，使得規(guī)劃問題發(fā)生在拓?fù)涔?jié)點表示的空間內(nèi)，避免了全局空間的規(guī)劃；同時，利用激光識別室內(nèi)環(huán)境中的門，并建立起房間之間的拓?fù)潢P(guān)系，再將該過程融入到粒子濾波SLAM(同時定位與建圖)算法中，最終得到環(huán)境表示的柵格地圖和基于柵格地圖的拓?fù)涞貓D，使得機器人導(dǎo)航中的路徑規(guī)劃算法更加簡單和高效。
【IPC分類】G05D1/02
【公開號】CN104898660
【申請?zhí)枴緾N201510140528
【發(fā)明人】陳贏峰, 陳小平
【申請人】中國科學(xué)技術(shù)大學(xué)
【公開日】2015年9月9日
【申請日】2015年3月27日