專利名稱：基于分層拓?fù)浣Y(jié)構(gòu)的多機(jī)器人系統(tǒng)地圖拼接方法
技術(shù)領(lǐng)域：
本發(fā)明屬于智能移動(dòng)機(jī)器人領(lǐng)域，涉及一種基于分層拓?fù)浣Y(jié)構(gòu)的未知環(huán)境下多機(jī)器人系統(tǒng)地圖拼接方法。
背景技術(shù)：
近年來，移動(dòng)機(jī)器人的應(yīng)用越來越普遍，各種具有較高智能的移動(dòng)機(jī)器人系統(tǒng)在不同場合正在發(fā)揮重要的作用。但隨著人類社會(huì)活動(dòng)領(lǐng)域的不斷延伸以及機(jī)器人學(xué)研究的不斷發(fā)展，單個(gè)移動(dòng)機(jī)器人系統(tǒng)在代替人完成包括大范圍災(zāi)難救援、科學(xué)考察等任務(wù)以及戰(zhàn)場環(huán)境監(jiān)察等軍事使命時(shí)面臨著諸多的困難，如:可靠性差、作業(yè)范圍小、任務(wù)完成效率低等。而相關(guān)研究表明，由多個(gè)移動(dòng)機(jī)器人組成的多機(jī)器人系統(tǒng)通過協(xié)調(diào)、合作恰恰可以解決這些問題。因此，多機(jī)器人系統(tǒng)被認(rèn)為具有廣闊的應(yīng)用前景。地圖拼接作為多機(jī)器人系統(tǒng)導(dǎo)航領(lǐng)域中的一項(xiàng)關(guān)鍵性技術(shù)，已引起了眾多學(xué)者的關(guān)注。在機(jī)器人初始位姿是已知的假設(shè)下，多機(jī)器人之間的地圖拼接問題可轉(zhuǎn)化為單機(jī)器人同步定位與地圖創(chuàng)建(Simultaneous Localization and Mapping, SLAM)問題的直接擴(kuò)展，然而針對大規(guī)模的未知環(huán)境，該假設(shè)存在著較大的局限性；在機(jī)器人必須相遇條件下，可借助于內(nèi)部傳感器信息計(jì)算地圖變換矩陣進(jìn)而實(shí)現(xiàn)地圖拼接，該方法需對傳感器信息不確定性進(jìn)行分析與優(yōu)化，且拼接地圖效果受限于傳感器檢測精度?？紤]到算法的通用性和魯棒性，解決機(jī)器人間相對位姿未知情況下的地圖拼接問題有著重要的意義。目前有學(xué)者利用相關(guān)掃描匹配策略搜索地圖公共區(qū)域來實(shí)現(xiàn)地圖拼接，取得了一些成果，解決方案是將局部地圖與局部地圖的數(shù)據(jù)關(guān)聯(lián)問題轉(zhuǎn)化為點(diǎn)與局部地圖的匹配問題，大大節(jié)省了搜索空間。Yan Yu等提出一種基于線段幾何特征的掃描匹配方法，在結(jié)構(gòu)化環(huán)境中取 得較好拼接結(jié)果。Zhou和Roumeliotis基于最鄰近搜索算法建立目標(biāo)函數(shù)，提出一種路標(biāo)匹配方法實(shí)現(xiàn)特征地圖融合。Lee H C和Lee B H.于2011年在《Advanced Robotics》雜志上發(fā)表的論文 “ Improved feature map merging usingVirtual Supporting Lines for mult1-Robot systems”,通過提取環(huán)境中的VSLs(VirtualSupporting Lines)非迭代特征進(jìn)行匹配來實(shí)現(xiàn)拼接,保證了地圖精度的同時(shí)降低算法復(fù)雜度。然而上述方法在缺少空間約束的環(huán)境中并不適用，且在大規(guī)模未知場景下，相關(guān)特征的錯(cuò)誤關(guān)聯(lián)會(huì)引入拼接誤差。隨著vSLAM(Visual SLAM)算法的日益成熟，一些學(xué)者嘗試將視覺特征融入系統(tǒng)以解決地圖拼接準(zhǔn)確性問題，但視覺特征提取實(shí)時(shí)性問題又是一個(gè)難題。

發(fā)明內(nèi)容
為了克服上述地圖拼接方法中存在的上述問題，本發(fā)明基于分層拓?fù)浣Y(jié)構(gòu)提供了一種視覺特征關(guān)聯(lián)與局部掃描匹配相結(jié)合的多機(jī)器人系統(tǒng)地圖拼接方法。本發(fā)明采用的技術(shù)方案如下:在機(jī)器人間相對位姿未知條件下，針對大規(guī)模的未知環(huán)境建立一種分層拓?fù)浣Y(jié)構(gòu)(Hierarchical Topology Map, HTM),利用基于視覺特征的拓?fù)涞貓D節(jié)點(diǎn)匹配方法,結(jié)合局部掃描匹配策略，提出一種快速魯棒的多機(jī)器人系統(tǒng)地圖拼接方法。該方法主要包括以下內(nèi)容:第一，提出融合可通行空間樹(Admissible Space Tree, AST)和尺度不變特征(Scale-1nvariant Feature Transform, SIFT)的分層拓?fù)浣Y(jié)構(gòu)，并利用一種改進(jìn)的同時(shí)路徑規(guī)劃與地圖創(chuàng)建(Simultaneous Path Planning and Topological Mapping, SP2ATM)方法建立未知環(huán)境的拓?fù)浣Y(jié)構(gòu)并增量式地創(chuàng)建地圖；第二，采取節(jié)點(diǎn)匹配策略關(guān)聯(lián)場景信息，利用SIFT匹配度作為地圖拼接的先驗(yàn)信息，并在遞增生成的拓?fù)涔?jié)點(diǎn)處設(shè)定關(guān)鍵幀(KeyFrame)，以降低匹配過程的計(jì)算復(fù)雜度；第三，在機(jī)器人間相對位姿未知情況下，結(jié)合迭代最近點(diǎn)(Iterative Closest Point, ICP)掃描匹配算法求取局部地圖間的變換矩陣，當(dāng)由ICP算法計(jì)算得到的地圖間相似度大于給定閾值時(shí)，完成多機(jī)器人系統(tǒng)的地圖拼接?；诜謱油?fù)浣Y(jié)構(gòu)的多機(jī)器人系統(tǒng)地圖拼接方法，包括以下步驟:步驟一，獲取可通行空間樹。為建立環(huán)境的拓?fù)浣Y(jié)構(gòu)并增量式地創(chuàng)建地圖，本發(fā)明首先根據(jù)傳感器信息建立環(huán)境中的可通行空間樹結(jié)構(gòu)，并實(shí)時(shí)獲取 機(jī)器人探索過程中的可能目標(biāo)點(diǎn)，進(jìn)而完成路徑規(guī)劃與地圖創(chuàng)建任務(wù)?？赏ㄐ锌臻g建立方法如下:利用獲取的激光數(shù)據(jù)信息，將距離機(jī)器人最近障礙物到激光最大量程的掃描區(qū)域劃分為若干層，每層包含若干段弧線，稱之為扇區(qū)。為確定在扇區(qū)中是否存在可能目標(biāo)點(diǎn)，本方法假設(shè)存在一個(gè)矩形通道，將大于機(jī)器人寬度的給定閾值w作為矩形的寬，并使該矩形以機(jī)器人當(dāng)前位置為中心進(jìn)行旋轉(zhuǎn)，在每個(gè)扇區(qū)中尋找可通行角度區(qū)間。在已建立可通行空間樹結(jié)構(gòu)中，為確定機(jī)器人的前進(jìn)方向和目標(biāo)點(diǎn)，選取最外層扇區(qū)中的弧線中間點(diǎn)，將其作為可能目標(biāo)點(diǎn)。這樣，機(jī)器人通過目標(biāo)點(diǎn)的實(shí)時(shí)更新完成環(huán)境探索任務(wù)。步驟二，建立分層拓?fù)浣Y(jié)構(gòu)。在可通行空間樹的基礎(chǔ)上建立一種融合混合特征的分層拓?fù)涞貓D結(jié)構(gòu)。在分層拓?fù)浣Y(jié)構(gòu)中維護(hù)了兩層地圖:第一層地圖是當(dāng)前節(jié)點(diǎn)的可通行空間樹的集合A ;第二層是已生成拓?fù)涔?jié)點(diǎn)集合T，稱為拓?fù)鋵?。同時(shí)，為獲取更豐富的環(huán)境信息，本發(fā)明在拓?fù)涔?jié)點(diǎn)處提取比例不變特征(Scale-1nvariant Feature Transform, SIFT),并存儲于分層拓?fù)涞貓D結(jié)構(gòu)當(dāng)中。機(jī)器人探索過程中，若已生成的可能目標(biāo)點(diǎn)與已生成拓?fù)涔?jié)點(diǎn)或者可能點(diǎn)間距離過于接近時(shí)，需利用節(jié)點(diǎn)更新規(guī)則對分層拓?fù)浣Y(jié)構(gòu)中的節(jié)點(diǎn)不斷進(jìn)行添加、綁定與刪除等動(dòng)態(tài)維護(hù)操作。步驟三，創(chuàng)建全局拓?fù)涞貓D。本發(fā)明基于上述的可能目標(biāo)點(diǎn)提取方法，不斷被提取新的目標(biāo)點(diǎn)，通過拓?fù)涔?jié)點(diǎn)的實(shí)時(shí)更新來完成全局拓?fù)涞貓D的創(chuàng)建。探索過程中，每個(gè)機(jī)器人建立自身的拓?fù)錁湫徒Y(jié)構(gòu)。機(jī)器人進(jìn)入未探索環(huán)境中，利用更新規(guī)則判斷當(dāng)前的可通行區(qū)域，若機(jī)器人當(dāng)前不存在任何可能目標(biāo)點(diǎn)時(shí)，為完成對環(huán)境的遍歷，機(jī)器人必須回溯到距離機(jī)器人最近且存在未探索可能目標(biāo)點(diǎn)的拓?fù)涔?jié)點(diǎn)處，并將此節(jié)點(diǎn)設(shè)置為回溯目標(biāo)。同時(shí)，為避免多機(jī)器人探索同一局部區(qū)域，每個(gè)機(jī)器人根據(jù)自身的分層拓?fù)涞貓D結(jié)構(gòu)和節(jié)點(diǎn)更新規(guī)則動(dòng)態(tài)維護(hù)結(jié)構(gòu)中所有節(jié)點(diǎn)，防止機(jī)器人間的重復(fù)訪問，以充分提高地圖創(chuàng)建效率。步驟四，提取SIFT特征并進(jìn)行特征匹配。SIFT特征對圖像的比例縮放、旋轉(zhuǎn)有著較好的不變性，并且對于三維視角、噪聲、光照的變化也保持著較好的可匹配性。因此，本發(fā)明通過提取環(huán)境中的SIFT特征角點(diǎn)來描述未知環(huán)境，并將節(jié)點(diǎn)間的SIFT匹配度作為地圖拼接的先驗(yàn)信息。本發(fā)明在拓?fù)涔?jié)點(diǎn)提取環(huán)境中的SIFT特征點(diǎn)，包括特征點(diǎn)檢測和特征點(diǎn)描述兩個(gè)部分。采用基于歐氏距離的最鄰近法進(jìn)行特征點(diǎn)匹配，利用K-D樹的數(shù)據(jù)結(jié)構(gòu)進(jìn)行搜索，根據(jù)最近兩個(gè)特征點(diǎn)的距離比確定是否接受這一匹配對。步驟五，基于ICP掃描匹配進(jìn)行地圖拼接。本發(fā)明基于上述分層拓?fù)浣Y(jié)構(gòu)完成機(jī)器人地圖創(chuàng)建，并對不同機(jī)器人在拓?fù)涔?jié)點(diǎn)處提取的SIFT特征進(jìn)行匹配，若匹配度滿足閾值要求，則對當(dāng)前不同機(jī)器人所生成的地圖使用ICP掃描匹配的方法來實(shí)現(xiàn)地圖拼接，無需考慮機(jī)器人相對位姿信息。拼接過程中，考慮ICP方法的匹配誤差，提高地圖拼接的準(zhǔn)確度。本發(fā)明的有益效果是:在機(jī)器人間相對位姿未知情況下，提出一種融合SIFT特征的分層拓?fù)浣Y(jié)構(gòu)，并增量式地創(chuàng)建全局拓?fù)涞貓D，根據(jù)節(jié)點(diǎn)間的SIFT匹配信息，結(jié)合掃描匹配的方法實(shí)現(xiàn)大規(guī)模未知環(huán)境下的多機(jī)器人系統(tǒng)地圖拼接。有效地解決了地圖創(chuàng)建效率問題，提高了拼接地圖的準(zhǔn)確度。


圖1為本發(fā)明所涉及的方法流程圖；圖2為可通行空間樹示意圖；圖3為分層拓?fù)浣Y(jié)構(gòu)示意圖；圖4為路徑探索方法流程圖；圖5為應(yīng)用本發(fā)明地圖拼接場景的實(shí)際環(huán)境示意圖:(a)為實(shí)驗(yàn)真實(shí)場景，(b)為實(shí)驗(yàn)環(huán)境的二維幾何示意圖；圖6為應(yīng)用本發(fā)明實(shí)現(xiàn)地圖拼接的結(jié)果示意圖。
具體實(shí)施例方式下面結(jié)合附圖和具體實(shí)施方式
對本發(fā)明作進(jìn)一步的詳細(xì)說明。本發(fā)明所述方法的流程圖如附圖1所示，包括以下幾個(gè)步驟:步驟一，獲取可通行空間樹，?？赏ㄐ锌臻g樹由可通行空間以及可能目標(biāo)點(diǎn)組成，其具體建立方法如下: (1)猶取扇區(qū)利用獲取的激光數(shù)據(jù)信息，將距離機(jī)器人最近障礙物到激光最大量程的掃描區(qū)域劃分為若干層，每層包含若干段弧線，弧線所圍成的區(qū)域稱之為扇區(qū)。(2)判斷可通行角度區(qū)間假設(shè)存在一個(gè)矩形通道，設(shè)定閾值w大于機(jī)器人寬度，并將w作為矩形的寬。使該矩形以機(jī)器人當(dāng)前位置為中心進(jìn)行旋轉(zhuǎn)，在每個(gè)扇區(qū)中尋找可通行角度區(qū)間。(3)選取可能目標(biāo)點(diǎn)為確定機(jī)器人的前進(jìn)方向和目標(biāo)點(diǎn)，選取最外層扇區(qū)中的可見點(diǎn)，將其作為當(dāng)前目標(biāo)點(diǎn)K，則機(jī)器人的可能目標(biāo)集合描述為:
權(quán)利要求
1.一種基于分層拓?fù)浣Y(jié)構(gòu)的多機(jī)器人系統(tǒng)地圖拼接方法，其特征在于包括以下幾個(gè)步驟: 步驟一，獲取可通行空間樹，方法如下: (1)獲取扇區(qū) 利用獲取的激光數(shù)據(jù)信息，將距離機(jī)器人最近障礙物到激光最大量程的掃描區(qū)域劃分為若干層，每層包含若干段弧線，弧線圍成一個(gè)個(gè)扇區(qū)； (2)判斷可通行角度區(qū)間 假設(shè)存在一個(gè)矩形通道，設(shè)定閾值w大于機(jī)器人寬度，并將w作為矩形的寬；使該矩形以機(jī)器人當(dāng)前位置為中心進(jìn)行旋轉(zhuǎn)，在每個(gè)扇區(qū)中尋找可通行角度區(qū)間； (3)選取可能目標(biāo)點(diǎn) 為確定機(jī)器人的前進(jìn)方向和目標(biāo)點(diǎn)，選取最外層扇區(qū)中的可見點(diǎn)，將其作為當(dāng)前目標(biāo)點(diǎn)P;，則機(jī)器人的可能目標(biāo)集合描述為: 式中，F(xiàn)是當(dāng)前可能目標(biāo)點(diǎn)隼P1是該點(diǎn)集中的第j個(gè)可能目標(biāo)點(diǎn)； 步驟二，建立分層拓?fù)浣Y(jié)構(gòu)，力法卯下: 在可通行空間樹的基礎(chǔ)上建立一種融合混合特征的分層拓?fù)涞貓D結(jié)構(gòu)；在分層拓?fù)浣Y(jié)構(gòu)H中維護(hù)兩層地圖:第一層地圖是當(dāng)前節(jié)點(diǎn)的可通行空間樹的集合A ;第二層是已生成拓?fù)涔?jié)點(diǎn)的集合T，即拓?fù)鋵?，分層拓?fù)浣Y(jié)構(gòu)H描述為:H = {A, T} 在拓?fù)涔?jié)點(diǎn)處提取比例不變特征(SIFT)，并融合到分層拓?fù)涞貓D結(jié)構(gòu)當(dāng)中；拓?fù)涔?jié)點(diǎn)集合Ti可描述為:Ti = {s，A，p} 式中，s是Ti處所提取的SIFT特征向量,A為Ti處建立的可能目標(biāo)點(diǎn)的集合，p表示機(jī)器人當(dāng)前位姿； 假設(shè)S為已生成的拓?fù)涔?jié)點(diǎn)，其可能目標(biāo)點(diǎn)集合為(Q1, Q2,..., Qj,..., Qn);當(dāng)前拓?fù)涔?jié)點(diǎn)為T，可能目標(biāo)點(diǎn)集合為(P1, P2,..., Pi,..., Pm)，其中N、M分別為S、T所含通路點(diǎn)個(gè)數(shù)；用d(P，Q)表示節(jié)點(diǎn)P與Q之間的距離；fflC(P，Q)表示節(jié)點(diǎn)P與Q之間的連通關(guān)系，其值為TRUE,表示P與Q連通；其值為FLASE，則表示P與Q間存在障礙；S = S/Qj表示將從拓?fù)涔?jié)點(diǎn)S中刪除，T = T U Qj表示將添加到拓?fù)涔?jié)點(diǎn)S中；具體動(dòng)態(tài)維護(hù)規(guī)則如下: (I)拓?fù)涔?jié)點(diǎn)T所屬區(qū)域\內(nèi)的可能目標(biāo)點(diǎn)Pi對機(jī)器人探索的作用不大，刪除Pi ;即: 若(KPiJ)彡 Vr，則 T = IyPi ； (2把落在已生成的拓?fù)涔?jié)點(diǎn)S所屬區(qū)域1，且？1與S間存在可通行路徑，刪除Pi ;即:若 d (Pi, S)彡 Vr，且 C (Pi, S) = TRUE,則 T = TZPi ； (3)若相鄰的通路點(diǎn)PpQj間不存在障礙物，且距離在閾值0內(nèi)，保留其中最有利于機(jī)器人探索的環(huán)境通路點(diǎn)；即:在(!(Pi, Qj) ( O 的情況下，若 d (Pi, T)彡 d(Qj，S)，則 T = TA3i ;否則 S = S/Qj ； (4)機(jī)器人探索過程中，需要對某些滿足條件的通路點(diǎn)重排列以保證有效的環(huán)境探索；即:若 d (Qj, T)≤ d (Qj, S)，且 C (Qj, T) = TRUE,則 S = S/Qj, T = T U Qj ； (5)若機(jī)器人運(yùn)動(dòng)到附近，且％與T間不存在障礙物，表明所代表未知區(qū)域已被探索，直接刪除％;即:若 d (Qj, T)≤ Vr，且 C (Qj, T) = TRUE,則 S = S/Qj ； 步驟三，創(chuàng)建全局拓?fù)涞貓D，方法如下: (1)探索開始時(shí)，首先獲取激光傳感器信息，根據(jù)激光數(shù)據(jù)對可能目標(biāo)點(diǎn)進(jìn)行搜索； (2)在整個(gè)環(huán)境中均不存在尚未探索的可能目標(biāo)點(diǎn)的情況下，若d(T，0)( 0，探索終止；否則，機(jī)器人回溯到初始點(diǎn)；其中T為當(dāng)前拓?fù)涔?jié)點(diǎn)，O為初始點(diǎn)，O為距離閾值； (3)當(dāng)前時(shí)刻提取的可能目標(biāo)點(diǎn)可能會(huì)有多個(gè)，采用效用函數(shù)U(P)這一評價(jià)標(biāo)準(zhǔn)，計(jì)算各個(gè)通路點(diǎn)的效用值，并選取效用函數(shù)值最大者作為即時(shí)目標(biāo)點(diǎn)，同時(shí)添加當(dāng)前拓?fù)涔?jié)點(diǎn)T并更新拓?fù)涞貓D；U(P)的表達(dá)式為:
全文摘要
本發(fā)明屬于智能移動(dòng)機(jī)器人領(lǐng)域，公開了一種基于分層拓?fù)浣Y(jié)構(gòu)的未知環(huán)境下多機(jī)器人系統(tǒng)地圖拼接方法。解決了在相對位姿未知情況下的多機(jī)器人系統(tǒng)地圖拼接問題。該方法包括獲取可通行空間樹，建立分層拓?fù)浣Y(jié)構(gòu)，創(chuàng)建全局拓?fù)涞貓D，提取SIFT特征并進(jìn)行特征匹配，基于ICP掃描匹配進(jìn)行地圖拼接。本發(fā)明在機(jī)器人間相對位姿未知的情況下，提出一種融合SIFT特征的分層拓?fù)浣Y(jié)構(gòu)，并增量式地創(chuàng)建全局拓?fù)涞貓D，根據(jù)節(jié)點(diǎn)間的SIFT匹配信息，結(jié)合掃描匹配的方法實(shí)現(xiàn)大規(guī)模未知環(huán)境下的多機(jī)器人系統(tǒng)地圖拼接。有效地提高了拼接的準(zhǔn)確性和實(shí)時(shí)性。該方法可適用于與地圖創(chuàng)建與地圖拼接有關(guān)的智能移動(dòng)機(jī)器人領(lǐng)域。
文檔編號G06T5/50GK103247040SQ20131017539
公開日2013年8月14日 申請日期2013年5月13日 優(yōu)先權(quán)日2013年5月13日
發(fā)明者賈松敏, 李雨晨, 王可, 李秀智 申請人:北京工業(yè)大學(xué)