專利名稱：多機(jī)器人三維幾何地圖的融合方法
技術(shù)領(lǐng)域：
本發(fā)明涉及機(jī)器人同時(shí)定位與地圖重建領(lǐng)域，具體來說是一種對(duì)多機(jī)器人所創(chuàng)建 的幾何地圖進(jìn)行融合的方法。
背景技術(shù)：
隨著應(yīng)用領(lǐng)域的不斷擴(kuò)大，基于單機(jī)器人系統(tǒng)的同時(shí)定位與地圖重建算法在很多 場(chǎng)合下已經(jīng)不能滿足需求；而多機(jī)器人系統(tǒng)由于具有并行作業(yè)、功能互補(bǔ)、信息共享等優(yōu) 點(diǎn)，受到了人們?cè)絹碓蕉嗟年P(guān)注。在移動(dòng)機(jī)器人同時(shí)定位與地圖重建問題中，地圖是根據(jù)機(jī) 器人的當(dāng)前位姿信息創(chuàng)建的，因此，如果能夠知道每個(gè)機(jī)器人的初始位姿信息及其對(duì)應(yīng)的 運(yùn)動(dòng)方程，那么就可以通過多機(jī)器人聯(lián)合濾波的方式來實(shí)現(xiàn)地圖的同時(shí)創(chuàng)建。然而，在很多 場(chǎng)合下，這個(gè)條件是不能得到滿足的，每個(gè)機(jī)器人都必須獨(dú)立的完成局部地圖的創(chuàng)建工作， 因而就需要面臨以下的一個(gè)問題當(dāng)每個(gè)機(jī)器人都維護(hù)了一張關(guān)于局部環(huán)境的地圖時(shí)，該 如何利用這些局部地圖以得到環(huán)境的全局信息？這就是多機(jī)器人的地圖融合問題(如圖1 所示)。針對(duì)機(jī)器人相互位置關(guān)系未知情況下的地圖融合問題，一種方法是通過機(jī)器人之 間的相互測(cè)量來估計(jì)地圖間的變換關(guān)系，然后對(duì)地圖中的特征進(jìn)行關(guān)聯(lián)，最終實(shí)現(xiàn)地圖的 融合；或者是機(jī)器人通過相互交換測(cè)量信息，然后采用一種自適應(yīng)粒子濾波算法來估計(jì)機(jī) 器人之間的相對(duì)位姿信息，并由此來決定多機(jī)器人協(xié)作探索環(huán)境的策略。針對(duì)多機(jī)器人柵 格地圖的融合問題，可以把它看作是求解最優(yōu)變換的過程，或者是通過圖像配準(zhǔn)的方式來 解決。然而，目前的方法大都針對(duì)的是二維地圖的融合問題。對(duì)三維幾何地圖的融合問題， 通常直接采用三維點(diǎn)集配準(zhǔn)算法來解決，但是三維點(diǎn)集配準(zhǔn)算法沒有考慮到地圖能夠反映 出環(huán)境中的幾何特征這一情況。

發(fā)明內(nèi)容
本發(fā)明提供了一種多機(jī)器人三維幾何地圖的融合方法，通過考慮三維地圖中的幾 何特征信息，結(jié)合二維地圖融合和三維點(diǎn)集配準(zhǔn)算法，完成多機(jī)器人三維幾何地圖的融合， 明顯了改進(jìn)三維幾何地圖融合的準(zhǔn)確性。一種多機(jī)器人三維幾何地圖的融合方法，包括以下步驟(1)將待融合的兩幅三維幾何地圖M1(^yd) *M2(x，y，z)投影到χ-y平面上降 維得到二維地圖，然后以固定的尺寸大小對(duì)二維地圖進(jìn)行柵格化得到待融合的兩幅柵格地 圖，其中，有特征存在的柵格為占據(jù)柵格，無特征存在的柵格則為空閑柵格，兩種不同的柵 格賦予不同的數(shù)值；(2)對(duì)待融合的兩幅柵格地圖進(jìn)行傅里葉-梅林變換，計(jì)算兩者之間的相對(duì)旋轉(zhuǎn) 角θ ；(3)利用步驟(2)得到的相對(duì)旋轉(zhuǎn)角θ，對(duì)三維幾何地圖M2 (X，y，ζ)進(jìn)行旋轉(zhuǎn)變 換，得到旋轉(zhuǎn)變換后的三維幾何地圖M' 2(x,y,z)；
3
(4)采用最鄰近迭代算法，對(duì)M1U, y，ζ)和M' 2(x, y，ζ)進(jìn)行融合。進(jìn)一步，所述的步驟⑷為(4. 1)計(jì)算M' 2(x,y,z)中每一點(diǎn)到M1 (x，y，ζ)的最近點(diǎn)，利用四元數(shù)算法，求解 使得M' 2(x, y，ζ)中的每一點(diǎn)和M1U, y，ζ)中對(duì)應(yīng)的最近點(diǎn)之間的歐拉距離平方和最小 的變換矩陣K ；(4.2)用所述的變換矩陣矩陣K對(duì)M' 2(x，y，ζ)進(jìn)行變換，得到更新的M' 2(x, y，ζ)，并計(jì)算M1O^y, ζ)和更新的M' 2(x,y,z)之間的距離，為一次迭代的結(jié)果；(4. 3)重復(fù)步驟(4. 1)和(4. 2)進(jìn)行迭代，計(jì)算兩次迭代的誤差，如果誤差大于 ε，繼續(xù)迭代過程；如果誤差小于ε，迭代結(jié)束。ε根據(jù)融合精度需要取值。本發(fā)明多機(jī)器人三維幾何地圖的融合方法，通過考慮三維地圖中的幾何特征信 息，結(jié)合二維地圖融合和三維點(diǎn)集配準(zhǔn)算法，完成多機(jī)器人三維幾何地圖的融合，明顯了改 進(jìn)三維幾何地圖融合的準(zhǔn)確性。本發(fā)明多機(jī)器人三維幾何地圖的融合方法主要用于在多機(jī) 器人同時(shí)定位與地圖重建應(yīng)用中，對(duì)單個(gè)機(jī)器人所創(chuàng)建的局部地圖進(jìn)行融合以形成全局地 圖。


圖1為多機(jī)器人三維幾何地圖融合問題的示意圖；圖2為本發(fā)明的多機(jī)器人三維幾何地圖的融合方法的流程圖；圖3為作為測(cè)試輸入的兩幅待融合的三維幾何地圖；圖4為圖3的測(cè)試輸入未融合前在二維平面上的投影；圖5為對(duì)圖3所示的測(cè)試輸入采用本發(fā)明方法融合后的測(cè)試結(jié)果在二維平面上的 投影；圖6為對(duì)圖3所示的測(cè)試輸入采用現(xiàn)有技術(shù)中的方法融合后的測(cè)試結(jié)果在二維平 面上的投影。
具體實(shí)施例方式下面結(jié)合實(shí)施例和附圖來詳細(xì)說明本發(fā)明，但本發(fā)明并不僅限于此。如圖2所示，一種多機(jī)器人三維幾何地圖的融合方法，對(duì)多個(gè)機(jī)器人用視覺傳感 器感知環(huán)境，并各自創(chuàng)建關(guān)于環(huán)境的局部三維幾何地圖進(jìn)行融合，其流程包括以下步驟考慮到機(jī)器人是在x-y平面上運(yùn)動(dòng)，定義降維映射(即投影模型)f (χ, y, ζ) — (X，y)其中(X，y，ζ)為幾何特征的空間坐標(biāo)，(x, y)為幾何特征的平面坐標(biāo)。將待融合的兩幅三維幾何地圖M1(^ya) *M2(x，y，z)投影到χ-y平面上降維，然 后以固定的尺寸大小對(duì)投影到χ-y平面上的地圖進(jìn)行柵格化得到待融合的兩幅柵格地圖 Hi1(Xjy) *m2(x，y)。柵格地圖由若干個(gè)相同尺寸大小的柵格單元組成，一個(gè)柵格單元對(duì)應(yīng) x-y平面上一個(gè)子區(qū)域，子區(qū)域的大小可以根據(jù)實(shí)際情況進(jìn)行調(diào)整。柵格地圖中，有特征存 在的柵格為占據(jù)柵格，無特征存在的柵格則為空閑柵格，兩種不同的柵格賦予不同的數(shù)值。設(shè)待融合的兩幅柵格地圖Hi1U, y) ^P m2 (x, y)中，m2(x, y)是通過對(duì)mjx，y)進(jìn) 行平移、旋轉(zhuǎn)和縮放變換后得到的，則Hi1(^y) *m2(x，y)存在以下如式(I)所示的變換關(guān)
4系m2 (χ, y) = Hi1 (s (xcos α +ysin α ) -χ0, s (-xsin α +ycos σ ) -y0)(I)式(I)中，s為縮放因子，α為旋轉(zhuǎn)角度，Xtl和％為平移量；則Hi1 (x, y)和m2 (x, y)對(duì)應(yīng)的傅里葉變換幅值F1 ( ξ，ζ )和F2 ( ξ，ζ )之間滿足 式(II)，如下所示
權(quán)利要求
一種多機(jī)器人三維幾何地圖的融合方法，其特征在于，包括以下步驟(1)將待融合的兩幅三維幾何地圖M1(x，y，z)，M2(x，y，z)投影到x y平面上降維得到二維地圖，然后以固定的尺寸大小對(duì)二維地圖進(jìn)行柵格化得到待融合的兩幅柵格地圖，其中，有特征存在的柵格為占據(jù)柵格，無特征存在的柵格則為空閑柵格，兩種不同的柵格賦予不同的數(shù)值；(2)對(duì)待融合的兩幅柵格地圖進(jìn)行傅里葉 梅林變換，計(jì)算兩幅柵格地圖之間的相對(duì)旋轉(zhuǎn)角θ；(3)利用步驟(2)得到的相對(duì)旋轉(zhuǎn)角θ，對(duì)三維幾何地圖M2(x，y，z)進(jìn)行旋轉(zhuǎn)變換，得到變換后的三維幾何地圖M′2(x，y，z)；(4)采用最鄰近迭代算法，對(duì)M1(x，y，z)和M′2(x，y，z)進(jìn)行融合。
2.如權(quán)利要求1所述的融合方法，其特征在于，所述的步驟(4)中，對(duì)M1U,y，ζ)和 M' 2(x, y，ζ)進(jìn)行融合的過程如下(4.1)計(jì)算M'2(x,y,z)中每一點(diǎn)到M1O^y, ζ)的最近點(diǎn)，利用四元數(shù)算法，求解使得 M' 2(x,y,z)中的每一點(diǎn)和M1O^y, ζ)中對(duì)應(yīng)的最近點(diǎn)之間的歐拉距離平方和最小的變換 矩陣K ；(4.2)用所述的變換矩陣矩陣K對(duì)M'2(x，y，z)進(jìn)行變換，得到更新的M' 2(x，y，z)， 并計(jì)算M1O^y, ζ)和更新的M' 2(x,y,z)之間的距離，為一次迭代的結(jié)果；(4. 3)重復(fù)步驟(4. 1)和(4. 2)，計(jì)算兩次迭代的誤差，如果誤差大于ε，則繼續(xù)重復(fù)上 述的步驟；如果誤差小于ε，則迭代結(jié)束。
全文摘要
本發(fā)明公開了一種多機(jī)器人三維幾何地圖的融合方法，包括把待融合的三維幾何地圖投影成二維的柵格地圖；采用圖像配準(zhǔn)算法對(duì)柵格地圖進(jìn)行融合，得到柵格地圖之間的旋轉(zhuǎn)參數(shù)；對(duì)三維地圖施行旋轉(zhuǎn)變換，采用三維點(diǎn)集配準(zhǔn)算法進(jìn)行三維幾何地圖的融合。本發(fā)明多機(jī)器人三維幾何地圖的融合方法，通過考慮三維地圖中的幾何特征信息，結(jié)合二維地圖融合和三維點(diǎn)集配準(zhǔn)算法，完成多機(jī)器人三維幾何地圖的融合，明顯了改進(jìn)三維幾何地圖融合的準(zhǔn)確性。本發(fā)明多機(jī)器人三維幾何地圖的融合方法主要用于在多機(jī)器人同時(shí)定位與地圖重建應(yīng)用中，對(duì)單個(gè)機(jī)器人所創(chuàng)建的局部地圖進(jìn)行融合以形成全局地圖。
文檔編號(hào)G06T17/00GK101944240SQ20101026207
公開日2011年1月12日 申請(qǐng)日期2010年8月20日 優(yōu)先權(quán)日2010年8月20日
發(fā)明者吉喆, 孟旭炯, 徐巍軍, 陳耀武 申請(qǐng)人:浙江大學(xué)