專利名稱：自身位置辨認方法和裝置以及三維形狀的計測方法和裝置的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種SI入外界的三維信息來辨認自身位置的自身位置 辨認方法和裝置以及三維形狀的計測方法和裝置。
背景技術(shù)：
作為直接辨認自身位置的辦法，已知有GPS、 DGPS、磁道釘 (magnetic marker )、磁方式、無線方式、圖像標(biāo)記(image marker)方 式、光學(xué)式/電磁式標(biāo)記方式等。但是這些辦法雖然有能進行穩(wěn)定的位 置辨認(identify)的優(yōu)點，但存在需要基礎(chǔ)設(shè)施、能進行位置檢測的 范圍有限、需要用于設(shè)置的工時等問題。對此，將不使用基礎(chǔ)設(shè)施而通過傳感器來計測周圍環(huán)境獲得的環(huán) 境信息、和新獲得的計測數(shù)據(jù)一邊比較一邊計算自身位置的自身位置 辨認辦法被各種各樣地提出。作為適合該目的的三維傳感器，例如可以舉出超聲波傳感器、圖 像傳感器、距離傳感器。但是，超聲波傳感器有計測范圍狹窄精度低 的問題，圖像傳感器有容易受到照明條件的影響的問題。與此相比，由于距離傳感器比超聲波傳感器能夠精度良好地計測 寬范圍的距離數(shù)據(jù)，而且，不像圖像處理那樣會受照明的變化的影響， 所以，有能夠不受外擾光影響地進行穩(wěn)定的計測的優(yōu)點。在非專利文獻1、專利文獻1等中提出了使用距離傳感器的位置辨 認的辦法。非專利文獻1的辦法是利用ICP算法一邊將使用三維掃描式激光 傳感器通過周圍環(huán)境計測而獲得的環(huán)境模型、和逐次得到的計測數(shù)據(jù) 進行比較一邊進行位置對準(zhǔn)(position matching; aligning )。ICP算法是對應(yīng)點非已知情況的位置對準(zhǔn)辦法之一，求出相對于先 前的計測數(shù)據(jù)的其后的計測數(shù)據(jù)的最鄰近的點， 一 邊進行旋轉(zhuǎn)/平移 (translate ) —邊以其距離的總和為最小的狀態(tài)變成 一 致狀態(tài)的方式來 求解(非專利文獻2)。專利文獻1的"自身位置辨認裝置"是專門無軌道且在外界沒有設(shè)置地面標(biāo)記等設(shè)備的裝置，是通過預(yù)先以手動使移動體沿作為目的的 軌道動作(教導(dǎo))，使外部環(huán)境的地圖自動記憶的裝置。在該專利文獻1中，如圖1所示那樣，使用外部環(huán)境地圖，該外 部環(huán)境地圖是將在平面內(nèi)移動的移動體101的、與該平面內(nèi)的周圍外部環(huán)境物體102的位置關(guān)系作為離散化的數(shù)據(jù)保持在存儲器上。所述 數(shù)據(jù)是以所述移動體上固定的點為原點103,包括離散化為規(guī)定間隔 的來自所述移動體的方向數(shù)據(jù)104、和在所述方向上的離散化為規(guī)定間 隔的到所述周圍外部環(huán)境物體的為止的距離數(shù)據(jù)105的組。此外，作為與本發(fā)明有關(guān)的技術(shù)，公開有非專利文獻3。 非專利文獻1: Andreas Nuchter, et al., "6D SLAM with an Application in Autonomous Mine Mapping", IEEE 2004 International Conference Robotics and Automation (ICRA '04), April 2004非專利文獻2: Paul J. Besl， "A Method for Registration of 3-D Shapes" , IEEE Transactions of Pattern Analysis and Mechanical Intelligence, Vol.14, No.2， February 1992非專利文獻3:關(guān)本清英等、"三維激光雷達的開發(fā)"，石川島播磨技報 Vol.43 No.4 (2003-7)專利文獻l:日本專利申請公開2003-15739號公報，"外部環(huán)境地圖以 及自身位置辨認裝置以及誘導(dǎo)控制裝置"發(fā)明內(nèi)容發(fā)明要解決的問題在使用三維激光雷達那樣的距離傳感器的情況下，被計測的三維 形狀上的被計測點成為離散到橫向以及垂直方向上的點群。該點群的 間隔，在距計測點的距離例如是50m的情況下，被計測點的間隔例如 達到在4黃向是大約315mm,在垂直方向是大約525mm。此外，在從多個計測位置來計測靜止的三維形狀的情況下，在三 維激光雷達那樣的距離傳感器中，每個計測位置，通常被計測點的位 置不同。進而，這樣的距離傳感器一般在測定距離上具有大約20cm前后的 誤差。因此，在使用三維激光雷達那樣的距離傳感器的情況下，有以下的限制條件A~C。條件A:計測數(shù)據(jù)中點數(shù)少(例如，l幀^66x50點)條件B:計測數(shù)據(jù)中包含誤差(例如，在測定距離中是大約20cm前后)條件C:計測數(shù)據(jù)未必測量相同的計測點。即，得到的距離數(shù)據(jù)是在橫向以及垂直方向上離散的點群，由于 每一計測的位置不同，所以沒有對應(yīng)點，在測定距離中包含比較大的 誤差。在使用所述"ICP算法，，來處理這樣的距離數(shù)據(jù)的情況下，有以下 問題點。(1 )誤差的累積ICP算法是兩個距離數(shù)據(jù)的重合辦法，反復(fù)比較先前的數(shù)據(jù)和其后 的數(shù)據(jù)，即使積分其差分，由于幾乎沒有對應(yīng)點，所以誤差累積。(2) 計算量多ICP算法由于是反復(fù)計算，所以計算量變得龐大。即，ICP算法由 于需要探測對應(yīng)于計測數(shù)據(jù)的各數(shù)據(jù)點的模型數(shù)據(jù)，所以當(dāng)模型數(shù)據(jù) 點數(shù)以及計測數(shù)據(jù)點數(shù)增加時計算量增大。具體是，在將模型數(shù)據(jù)的 點數(shù)作為M,計測數(shù)據(jù)的點數(shù)作為N的情況下，例如全探測時的計算 量級(order)成為O(MxN)。(3) 不能處理計測點少的情況由于ICP算法以密集的距離數(shù)據(jù)為對象，所以在是離散的點群， 在空間上稀疏的情況下，會收斂到錯誤的結(jié)果。因此，使用了這樣的距離傳感器的位置辨認，需要滿足以下的要求。(1)存儲器效率良好的用于周圍環(huán)境的數(shù)據(jù)結(jié)構(gòu)將逐次得到的計測數(shù)據(jù)全部保存的方法中需要無窮盡的存儲器。 因此，需要將計測的周圍環(huán)境結(jié)果高效存放的數(shù)據(jù)結(jié)構(gòu)。 (2 )對計測數(shù)據(jù)的點數(shù)不足以及誤差的穩(wěn)定性即使在計測數(shù)據(jù)的點數(shù)少的情況下以及包含誤差的情況下，也需 要盡量不使位置辨認精度下降。 (3)用于位置辨認的計算的高效化通過將計測周圍環(huán)境而獲得的環(huán)境信息和計測數(shù)據(jù)比較，從而進 行自身位置辨認，但該比較處理需要很多的計算。發(fā)明的概要本發(fā)明是為了解決上述問題而做出的。即，本發(fā)明的目的是提供 一種自身位置辨認方法和裝置以及三維形狀的計測方法和裝置，其中 具有存儲器效率良好的用于周圍環(huán)境的數(shù)據(jù)結(jié)構(gòu)，對于計測數(shù)據(jù)的點 數(shù)不足以及誤差有穩(wěn)定性，用于位置辨認的計算能夠高效進行，由此， 即使在外界的三維信息是空間離散的點群，每個計測的位置不同，沒 有對應(yīng)點，包含比較大的誤差的情況下，也能夠沒有誤差的累積，并 能以少的計算量進行正確的位置對準(zhǔn)。根據(jù)本發(fā)明，提供一種自身位置辨認方法，引入外界的三維形狀來辨認自身位置，其特征在于，實施下述步驟數(shù)據(jù)輸入步驟，在新 的計測位置處將三維形狀上的坐標(biāo)值輸入到計算機中；模型構(gòu)筑步驟， 構(gòu)筑環(huán)境模型，該環(huán)境模型將所述三維形狀所存在的空間區(qū)域分割為 邊界表面相互正交的長方體構(gòu)成的多個體素(voxel )，并存儲各體素位 置；以及匹配步驟，在對應(yīng)于所述坐標(biāo)值的體素的內(nèi)部設(shè)定代表點及 其誤差分布并存儲起來，在先前的計測位置的所述三維形狀數(shù)據(jù)不存 在的情況下，將新的計測位置辨認為自身位置，在先前的計測位置的 所述三維形狀數(shù)據(jù)存在的情況下，實施下述步驟精密對準(zhǔn)步驟，對 于針對先前的計測位置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋 轉(zhuǎn)及平移，或者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與 鄰近的誤差分布間距離有關(guān)的評價值(例如，該距離的總和)成為最 小的方式，進行位置對準(zhǔn)；以及自身位置辨認步驟，根據(jù)所述精密對 準(zhǔn)步驟中的旋轉(zhuǎn)量以及平移量來辨認自身位置。再有，進而實施輸出步驟，將所述自身位置輸出到輸出裝置中也可。此外，根據(jù)本發(fā)明，提供一種三維形狀的計測方法，根據(jù)外界的 三維形狀的被計測點的坐標(biāo)值來復(fù)原三維形狀，并輸出三維形狀數(shù)據(jù)， 其特征在于，實施下述步驟數(shù)據(jù)輸入步驟，在新的計測位置處將三 維形狀上的坐標(biāo)值輸入到計算機中；模型構(gòu)筑步驟，構(gòu)筑環(huán)境模型， 該環(huán)境模型將所述三維形狀所存在的空間區(qū)域分割為由邊界表面相互 正交的長方體構(gòu)成的多個體素，并存儲各體素位置；以及匹配步驟， 在對應(yīng)于所述坐標(biāo)值的體素的內(nèi)部設(shè)定代表點及其誤差分布并存儲起 來，在先前的計測位置的所述三維形狀數(shù)據(jù)不存在的情況下，將新的 計測位置辨認為自身位置，在先前的計測位置的所述三維形狀數(shù)據(jù)存在的情況下，實施下述步驟精密對準(zhǔn)步驟，對于針對先前的計測位 置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，或者使針 對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與鄰近的誤差分布間距 離有關(guān)的評價值成為最小的方式進行位置對準(zhǔn)；以及自身位置辨認步 驟，根據(jù)所述精密對準(zhǔn)步驟中的旋轉(zhuǎn)量以及平移量來辨認自身位置， 還具有輸出步驟，將所述自身位置、基于所述自身位置的所述體素 位置、代表點以及誤差分布的至少任一個輸出到輸出裝置中。再有，在所述自身位置辨認方法或三維形狀計測方法中，關(guān)于上 述距離的評價值，替代該距離的總和，是該距離的平均值、該距離的 平方的總和、或該距離的最大值也可，是其他的適合的評價值也可。所述自身位置辨認方法或三維形狀的計測方法，包含以下的內(nèi)容 也可。根據(jù)本發(fā)明的優(yōu)選實施方式，在所述自身位置辨認步驟中，根據(jù) 先前的計測位置的位置以及姿勢來辨認新的計測位置的六自由度位 置。此外，在所述匹配步驟中，體素內(nèi)部除了代表點及其誤差分布之 外，還設(shè)定并存儲表示在體素內(nèi)物體存在概率的概率值。此外，在所述精密對準(zhǔn)步驟之前具有粗略對準(zhǔn)步驟，對于針對 先前的計測位置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平 移，并以與具有代表點的體素間距離有關(guān)的評價值(例如，該距離的 總和)成為最小的方式，進行位置對準(zhǔn)，其中該代表點是與計測數(shù)據(jù) 以及誤差分布鄰近的代表點，或者使針對新的計測位置的環(huán)境模型旋 轉(zhuǎn)及平移，并以與具有代表點的體素間距離有關(guān)的評價值(例如，該 距離的總和)成為最小的方式，進行位置對準(zhǔn)。再有，與具有和計測數(shù)據(jù)以及誤差分布鄰近的代表點的體素間距離有關(guān)的上述評價值，代替該距離的總和，是該距離的平均值、該距 離的二次方的總和、或該距離的最大值也可，是其他的適合的評價值 也可。此外，與具有代表點的體素間距離有關(guān)的上述評價值，代替該距 離的總和,是該距離的平均值、該距離的二次方的總和、或該距離的 最大值也可，是其他的適合的評價值也可?；蛘?，在所述匹配步驟中，具有設(shè)定并存儲表示在體素的內(nèi)部物體存在概率的概率值的單元的情況下，在所述精密對準(zhǔn)步驟之前具有 粗略對準(zhǔn)步驟，對于針對先前的計測位置的環(huán)境;f莫型，使新的計測數(shù) 據(jù)以及誤差分布旋轉(zhuǎn)及平移，并以與具有代表點的體素的概率值有關(guān) 的評價值(例如，該概率值的總和)成為最大的方式，進行位置對準(zhǔn)， 其中該代表點是與計測數(shù)據(jù)以及誤差分布鄰近的代表點，或者使針對 新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與鄰近的體素所具有的概 率值的差有關(guān)的評價值(例如，該概率值的差的總和)成為最小的方 式進行位置對準(zhǔn)。再有，與具有和計測數(shù)據(jù)以及誤差分布鄰近的代表點的體素的概率值有關(guān)的上述評價值，代替該概率值的總和，是該概率值的平均值、 該概率值的二次方的總和、或該概率值的最小值也可，是其他的適合 的評^階值也可。此外，與鄰近的體素所具有的概率值的差有關(guān)的上述評價值，代 替該概率值的差的總和，是該概率值的差的平均值、該概率值的差的 二次方的總和、或該概率值的差的最大值也可，是其他的適合的評價 值也可。此外，在所述數(shù)據(jù)輸入步驟之后具有探測范圍限定步驟，根據(jù) 過去的計測位置的變化推定當(dāng)前的計測位置，或通過能夠取得當(dāng)前的 計測位置的傳感器來取得，或不僅利用計測數(shù)據(jù)的距離值還利用反射 強度值，限定核對的范圍。此外，在所述精密對準(zhǔn)步驟中，將誤差分布交叉的情況作為同一 計測點，將根據(jù)分布的一致度求得的加權(quán)(weight)乘以該情況的距離 值，計算誤差分布間的距離。此外，在所述模型構(gòu)筑步驟中，在將最大的體素設(shè)定為相當(dāng)于必 要最小限度的分辨能力的大小，而且單一的體素內(nèi)存在多個被計測點 的情況下，以在單一的體素內(nèi)僅存在單一的被計測點的方式，將該體 素進一步分割并分級地分割成多個體素。此外，在所述自身位置辨認步驟之后具有更新所述環(huán)境模型的模 型更新步驟，在該模型更新步驟中，探測對應(yīng)于新輸入的被計測點的 坐標(biāo)值的體素，作為在原點和;波計測點之間不存在物體的，再設(shè)定或 擦除位于其間的體素內(nèi)的代表點和誤差分布。此外，在所述自身位置辨認步驟之后具有更新所述環(huán)境模型的模型更新步驟，在該才莫型更新步驟中，探測對應(yīng)于新輸入的被計測點的 坐標(biāo)值的體素，在該體素內(nèi)沒有代表點的情況下，將所述坐標(biāo)值和誤 差分布設(shè)定為代表點的坐標(biāo)值和誤差分布。此外，在所述自身位置辨認步驟之后具有更新所述環(huán)境模型的模 型更新步驟，在該模型更新步驟中，探測對應(yīng)于新輸入的被計測點的 坐標(biāo)值的體素，在所述體素內(nèi)有已經(jīng)設(shè)定了的代表點的情況下，比較 新取得的誤差分布和已經(jīng)設(shè)定了的體素內(nèi)的誤差分布，在誤差分布相 互重疊的情況下，根據(jù)兩誤差分布、或根據(jù)兩誤差分布和體素內(nèi)已經(jīng) 設(shè)定了的代表點和新輸入的被計測點的坐標(biāo)值，再設(shè)定新的誤差分布 和新的代表點，在誤差分布不相互重疊的情況下，以在單一的體素內(nèi) 僅存在單 一 的代表點的方式，將該體素進一 步分割并分級地分割為多個體素。體素的分割例如使用八叉樹或K-D樹。此外，將自身位置的誤差分布與自身位置的辨認一起進行辨認， 根據(jù)當(dāng)前的自身位置和誤差分布以及辨認了的自身位置和誤差分布通 過卡爾曼濾波器，校正自身位置和誤差分布，輸出自身位置和誤差分 布。在所述模型更新步驟中，比較所述新取得的誤差分布和所述已經(jīng) 設(shè)定了的體素內(nèi)的誤差分布，在誤差分布相互重疊的情況下，根據(jù)兩 誤差分布再設(shè)定新的誤差分布和新的代表點，其結(jié)果是新的代表點移 動到其他體素內(nèi)，此時，在該其他體素內(nèi)沒有代表點的情況下，在該 其他體素的內(nèi)部設(shè)定該新的誤差分布和該新的代表點，在該其他體素 內(nèi)有已經(jīng)設(shè)定了的代表點的情況下，比較該新的誤差分布和已經(jīng)設(shè)定 了的該其他體素內(nèi)的誤差分布，(A)在誤差分布相互重疊的情況下， 根據(jù)兩誤差分布、或根據(jù)兩誤差分布和體素內(nèi)已經(jīng)設(shè)定了的代表點和 新輸入的被計測點的坐標(biāo)值，再設(shè)定新的誤差分布和新的代表點，(B) 在誤差分布不相互重疊的情況下，以在單一的體素內(nèi)僅存在單一的代 表點的方式，將該體素進一 步分割并分級地分割為多個體素。此外，在所述自身位置辨認步驟之后具有更新所述環(huán)境模型的模 型更新步驟，在該模型更新步驟中，根據(jù)新輸入的被計測點的坐標(biāo)值 及其誤差分布、和已經(jīng)設(shè)定了的體素內(nèi)的代表點及其誤差分布，通過 卡爾曼濾波器取得新的代表點和誤差分布并進行再設(shè)定。根據(jù)本發(fā)明的其他的實施方式，在所述精密對準(zhǔn)步驟中，代替以與所述鄰近的誤差分布間距離有關(guān)的評價值成為最小的方式進行位置 對準(zhǔn)，而是以與一致度有關(guān)的評價值(例如，該一致度的總乘積)成 為最大的方式，對于針對先前的計測位置的環(huán)境模型，使新的計測數(shù) 據(jù)以及誤差分布旋轉(zhuǎn)及平移，或者使針對新的計測位置的環(huán)境模型旋 轉(zhuǎn)及平移，從而進行位置對準(zhǔn)，其中該一致度是通過基于所述鄰近的 誤差分布間的極大似然推定值而確定的。所述與 一致度有關(guān)的評價值的計算式以下面的[算式1 ]表示，[算式l]在該式中，計測點j和環(huán)境模型上的代表點i對應(yīng)起來，能得到該 計測點j的計測數(shù)據(jù)的概率為EM (i， j)， (D(j)在環(huán)境^t型中與計測點 j對應(yīng)起來的代表點存在的情況下是1,在除此之外的情況下為0。在所述輸出步驟中，將所述體素位置、代表點位置以及誤差分布 位置的至少任 一 個作為三維形狀的計測值輸出到輸出裝置中，并且將 表示該計測值的可靠性或精度的指標(biāo)，基于該體素內(nèi)部的誤差分布的 大小，輸出到輸出裝置中。在所述輸出步驟中，將所述體素位置、代表點位置以及誤差分布 位置的至少任 一 個作為三維形狀的計測值輸出到輸出裝置中時，在該 體素內(nèi)部的誤差分布的大小比規(guī)定的基準(zhǔn)值大的情況下，該計測值的 可靠性或精度定為比規(guī)定的基準(zhǔn)低，不將該體素的所述計測值輸出到 輸出裝置中。在所述匹配步驟之后具有更新所述環(huán)境模型的模型更新步驟，在 該模型更新步驟中，探測對應(yīng)于新輸入的被計測點的坐標(biāo)值的體素， 在所述體素內(nèi)的代表點以及誤差分布的至少任一個被新設(shè)定、或被再 設(shè)定、或?qū)⒃擉w素進一步分割并分級地分割為多個體素的情況下，在 所述輸出步驟中，將該體素的代表點位置作為三維形狀的計測值輸出 到輸出裝置中。在所述輸出步驟中，將從距離傳感器的位置出發(fā)距離傳感器能進 行位置計測的范圍的環(huán)境模型內(nèi)的體素的代表點位置，作為三維形狀的計測值輸出到輸出裝置中。此外，根據(jù)本發(fā)明，提供一種自身位置辨認裝置，引入外界的三 維形狀來辨認自身位置，其特征在于，具備數(shù)據(jù)輸入裝置，將三維形狀上的坐標(biāo)值輸入到計算機中；模型構(gòu)筑裝置，構(gòu)筑環(huán)境模型，該環(huán)境模型將所述三維形狀所存在的空間區(qū)域分割為由邊界表面相互正交的長方體構(gòu)成的多個體素，并存儲各體素位置；匹配裝置，在對應(yīng) 于所述坐標(biāo)值的體素的內(nèi)部設(shè)定代表點及其誤差分布并存儲起來；以 及數(shù)據(jù)傳輸裝置，將自身位置輸出到輸出裝置中，在新的計測位置處， 在先前的計測位置的所述三維形狀的數(shù)據(jù)不存在的情況下，將新的計 測位置辨認為自身位置，在新的計測位置處，在先前的計測位置的所 述三維形狀數(shù)據(jù)存在的情況下，對于針對先前的計測位置的環(huán)境模型， 使針對新的計須'J位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與鄰近的誤差分布 間距離有關(guān)的評價值(例如，該距離的總和)成為最小的方式，進行 位置對準(zhǔn)，根據(jù)所述位置對準(zhǔn)中的旋轉(zhuǎn)量以及平移量來辨認自身位置。此外，根據(jù)本發(fā)明，提供一種三維形狀的計測裝置，根據(jù)三維形 狀上的被計測點的坐標(biāo)值來復(fù)原三維形狀，并輸出三維形狀數(shù)據(jù)，其 特征在于，具備數(shù)據(jù)輸入裝置，將三維形狀上的坐標(biāo)值輸入到計算 機中；模型構(gòu)筑裝置，構(gòu)筑環(huán)境模型，該環(huán)境模型將所述三維形狀所 存在的空間區(qū)域分割為由邊界表面相互正交的長方體構(gòu)成的多個體 素，并存儲各體素位置；匹配裝置，在對應(yīng)于所述坐標(biāo)值的體素的內(nèi) 部設(shè)定代表點及其誤差分布并存儲起來；以及數(shù)據(jù)傳輸裝置，其中， 在數(shù)據(jù)傳輸裝置中，在新的計測位置處，在先前的計測位置的所述三 維形狀數(shù)據(jù)不存在的情況下，將新的計測位置辨認為自身位置，在新 的計測位置處，在先前的計測位置的所述三維形狀數(shù)據(jù)存在的情況下， 對于針對先前的計測位置的環(huán)境模型，使針對新的計測位置的環(huán)境模 型旋轉(zhuǎn)及平移，并以與鄰近的誤差分布間距離有關(guān)的評價值成為最小 的方式，進行位置對準(zhǔn)，根據(jù)所述位置對準(zhǔn)中的旋轉(zhuǎn)量以及平移量來 辨認自身位置，進而將所述自身位置、基于所述自身位置的所述體素 位置、代表點以及誤差分布的至少任 一 個輸出到輸出裝置中。所述自身位置辨認裝置或三維形狀的計測裝置，包含以下的內(nèi)容 也可。所述匹配裝置在體素的內(nèi)部除了代表點及其誤差分布之外，還設(shè)定并存儲表示在體素內(nèi)物體存在概率的概率值。所述自身位置辨認裝置或三維形狀的計測裝置，在所述位置對準(zhǔn)(精密對準(zhǔn)步驟)之前，進行粗略對準(zhǔn)步驟，對于針對先前的計測位置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，并以與 具有代表點的體素間距離有關(guān)的評價值(例如，該距離的總和)成為 最小的方式，進行位置對準(zhǔn)，其中該代表點是與計測數(shù)據(jù)以及誤差分 布鄰近的代表點，或者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移， 并以與具有代表點的體素間距離有關(guān)的評價值(例如，該距離的總和) 成為最小的方式，進行位置對準(zhǔn)。所述自身位置辨認裝置或三維形狀的計測裝置，在所述精密對準(zhǔn)步驟之前，進行粗略對準(zhǔn)步驟，對于針對先前的計測位置的環(huán)境沖莫 型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，并以與具有代表點的 體素的概率值有關(guān)的評價值(例如，該概率值的總和)成為最大的方 式，進行位置對準(zhǔn)，其中該代表點是與計測數(shù)據(jù)以及誤差分布鄰近的 代表點，或者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與鄰 近的體素所具有的概率值的差有關(guān)的評價值(例如，該概率值的差的 總和)成為最小的方式，進行位置對準(zhǔn)。所述自身位置辨認裝置或三維形狀的計測裝置，在所述數(shù)據(jù)輸入 步驟之后，根據(jù)過去的計測位置的變化推定當(dāng)前的計測位置，或通過 能夠取得當(dāng)前的計測位置的傳感器來取得，或不僅利用計測數(shù)據(jù)的距 離值還利用反射強度值，限定核對的范圍。所述自身位置辨認裝置或三維形狀的計測裝置，據(jù)先前的計測位 置的位置以及姿勢來辨認新的計測位置的六自由度位置。所述自身位置辨認裝置或三維形狀的計測裝置，將誤差分布交叉 的情況作為同一計測點，將根據(jù)分布的一致度求得的加權(quán)乘以該情況 的距離值，計算誤差分布間的距離。所述模型構(gòu)筑裝置在將最大的體素設(shè)定為相當(dāng)于必要最小限度的 分辨能力的大小，而且單一的體素內(nèi)存在多個被計測點的情況下，以 在單一的體素內(nèi)僅存在單一的被計測點的方式，將該體素進一步分割 并分級地分割成多個體素。所述自身位置辨認裝置或三維形狀的計測裝置，在所述自身位置 的辨認(自身位置辨認步驟)之后具有更新所述環(huán)境模型的模型更新裝置，該模型更新裝置探測對應(yīng)于新輸入的被計測點的坐標(biāo)值的體素， 作為在原點和被計測點之間不存在物體的，再設(shè)定或擦除位于其間的 體素內(nèi)的代表點和誤差分布。所述自身位置辨認裝置或三維形狀的計測裝置，在所述自身位置 辨認步驟之后具有更新所述環(huán)境模型的模型更新裝置，該模型更新裝 置探測對應(yīng)于新輸入的被計測點的坐標(biāo)值的體素，在該體素內(nèi)沒有代 表點的情況下，將所述坐標(biāo)值和誤差分布設(shè)定為代表點的坐標(biāo)值和誤 差分布。所述自身位置辨認裝置或三維形狀的計測裝置，在所述自身位置 辨認步驟之后具有更新所述環(huán)境模型的模型更新步驟，在該模型更新 步驟中，探測對應(yīng)于新輸入的一皮計測點的坐標(biāo)值的體素，在所述體素 內(nèi)有已經(jīng)設(shè)定了的代表點的情況下，比較新取得的誤差分布和已經(jīng)設(shè) 定了的體素內(nèi)的誤差分布，在誤差分布相互重疊的情況下，根據(jù)兩誤 差分布、或根據(jù)兩誤差分布和體素內(nèi)已經(jīng)設(shè)定了的代表點和新輸入的;故計測點的坐標(biāo)值，再設(shè)定新的誤差分布和新的代表點，在誤差分布 不相互重疊的情況下，以在單一的體素內(nèi)僅存在單一的代表點的方式， 將該體素進一步分割并分級地分割為多個體素。所述自身位置辨認裝置或三維形狀的計測裝置，在所述自身位置 辨認步驟中，在自身位置辨認的同時，辨認自身位置的誤差分布，在 所述輸出步驟之前，根據(jù)當(dāng)前的自身位置和誤差分布以及辨認了的自 身位置和誤差分布，利用卡爾曼濾波器，校正自身位置和誤差分布， 所述數(shù)據(jù)傳輸裝置將自身位置和誤差分布輸出到輸出裝置中。所述模型更新裝置比較所述新取得的誤差分布和所述已經(jīng)設(shè)定了 的體素內(nèi)的誤差分布，在誤差分布相互重疊的情況下，根據(jù)兩誤差分 布再設(shè)定新的誤差分布和新的代表點，其結(jié)果是新的代表點移動到其 他體素內(nèi)時，在該其他體素內(nèi)沒有代表點的情況下，在該其他體素的 內(nèi)部設(shè)定該新的誤差分布和該新的代表點，在該其他體素內(nèi)有已經(jīng)設(shè) 定了的代表點的情況下，比較該新的誤差分布和已經(jīng)設(shè)定了的該其他 體素內(nèi)的誤差分布，(A)在誤差分布相互重疊的情況下，根據(jù)兩誤差 分布、或根據(jù)兩誤差分布和體素內(nèi)已經(jīng)設(shè)定了的代表點和新輸入的被 計測點的坐標(biāo)值，再設(shè)定新的誤差分布和新的代表點，(B)在誤差分 布不相互重疊的情況下，以在單一的體素內(nèi)僅存在單一的代表點的方式，將該體素進一步分割并分級地分割為多個體素。所述自身位置辨認裝置或三維形狀的計測裝置，在所述自身位置 辨認步驟之后具有更新所述環(huán)境模型的模型更新裝置，該模型更新裝 置根據(jù)新輸入的被計測點的坐標(biāo)值、和已經(jīng)設(shè)定了的體素內(nèi)的代表點 及其誤差分布，通過卡爾曼濾波器取得新的代表點和誤差分布并進行 再設(shè)定。所述自身位置辨認裝置或三維形狀的計測裝置，在所述精密對準(zhǔn) 步驟中，代替以與所述鄰近的誤差分布間距離有關(guān)的評價值(例如， 距離的總和)成為最小的方式進行位置對準(zhǔn)，而是以與一致度有關(guān)的 評價值(例如，該一致度的總乘積)成為最大的方式，對于針對先前 的計測位置的環(huán)境模型，使新的計觀'j數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移， 或者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，從而進行位置對準(zhǔn)， 其中該 一致度是通過基于所述鄰近的誤差分布間的極大似然推定值而 確定的。在該情況下，所述與一致度有關(guān)的評價值的計算式以所述[算 式l]表示。所述數(shù)據(jù)傳輸裝置將所述體素位置、代表點位置以及誤差分布位 置的至少任一個作為三維形狀的計測值輸出到輸出裝置中，并且將表 示該計測值的可靠性或精度的指標(biāo)，基于該體素內(nèi)部的誤差分布的大 小，輸出到輸出裝置中。所述數(shù)據(jù)傳輸裝置將所述體素位置、代表點位置以及誤差分布位 置的至少任 一 個作為三維形狀的計測值輸出到輸出裝置中時，在該體 素內(nèi)部的誤差分布的大小比規(guī)定的基準(zhǔn)值大的情況下，該計測值的可 靠性或精度定為比規(guī)定的基準(zhǔn)低，不將該體素的所述計測值輸出到輸 出裝置中。所述數(shù)據(jù)輸入裝置使用距離傳感器將三維形狀上的坐標(biāo)值作為以 任意計測位置為原點的距離數(shù)據(jù)， 一 邊移動原點 一 邊依次取得。在所述匹配裝置之后具有更新所述環(huán)境模型的模型更新裝置，在 該模型更新裝置中，探測對應(yīng)于新輸入的被計測點的坐標(biāo)值的體素， 在所述體素內(nèi)的代表點以及誤差分布的至少任 一 個被新設(shè)定、或被再 設(shè)定、或?qū)⒃擉w素進一步分割并分級地分割為多個體素的情況下，所 述輸出裝置，將該體素的代表點位置作為三維形狀的計測值輸出到輸 出裝置中。所述數(shù)據(jù)傳輸裝置將從距離傳感器的位置出發(fā)距離傳感器能進行 位置計測的范圍的環(huán)境模型內(nèi)的體素的代表點位置，作為三維形狀的 計測值輸出到輸出裝置中。發(fā)明的效果根據(jù)所述本發(fā)明的方法和裝置，由于將三維形狀所存在的空間區(qū) 域，分割為多個體素，并存儲各體素位置，所以即使是計測對象物很 大的情況下，也能夠?qū)?shù)據(jù)量抑制在與體素數(shù)成比例的小的數(shù)據(jù)尺寸。誤差分布，所以能夠表現(xiàn):素的分辨能力以丄的信息。 ' 、因此，通過本發(fā)明的數(shù)據(jù)結(jié)構(gòu)，能夠?qū)⒍鄠€視點的計測數(shù)據(jù)綜合 為固定大小。此外，由于通過設(shè)定、保存表示在體素的內(nèi)部物體的存在概率的 概率值，從而即使在誤差分布比代表點所屬的體素擴展的情況下，也 不用找到代表點所屬的體素，并從其誤差分布再計算各體素中的物體 的存在有無，而是僅以該體素的概率值就能容易地判斷，所以能夠抑 制檢索時間。此外，由于根據(jù)過去的計測位置的變化推定當(dāng)前的計測位置，或 通過能夠取得當(dāng)前的計測位置的傳感器來取得，或不僅利用計測數(shù)據(jù) 的距離值還利用反射強度值，限定核對的范圍，所以能夠抑制檢索時 間。此外，由于在粗略對準(zhǔn)步驟中，對于針對先前的計測位置的環(huán)境 模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，并以與具有代表點 的體素間距離有關(guān)的評價值(例如，該距離的總和)成為最小的方式， 進行位置對準(zhǔn)，其中該代表點是與計測數(shù)據(jù)以及誤差分布鄰近的代表 點，或者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與具有代 表點的體素間距離有關(guān)的評價值(例如，該距離的總和)成為最小的 方式，進行位置對準(zhǔn)，或者，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平 移，并以與具有代表點的體素的概率值有關(guān)的評價值(例如，該概率 值的總和)成為最大的方式，進行位置對準(zhǔn)，其中該代表點是與計測 數(shù)據(jù)以及誤差分布鄰近的代表點，或者使針對新的計測位置的環(huán)境才莫 型旋轉(zhuǎn)及平移，并以與鄰近的體素所具有的概率值的差有關(guān)的評價值 (例如，該概率值的差的總和)成為最小的方式，進行位置對準(zhǔn)，所以能夠 一邊防止誤差的累積， 一邊短時間地進行具有代表點的體素彼 此的位置對準(zhǔn)。此外，由于對于針對先前的計測位置的環(huán)境模型，使針對新的計 測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與鄰近的體素所具有的概率值的 差有關(guān)的評價值(例如，該概率差的總和)成為最小的方式，進行位 置對準(zhǔn)，在該情況下，附加不存在物體的信息進行位置對準(zhǔn)，所以能 夠謀求精度的提高。接著，由于在精密對準(zhǔn)步驟中，對于針對先前的計測位置的環(huán)境 模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，或者使針對新的計 測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與鄰近的誤差分布間距離有關(guān)的 評價值(例如，該距離的總和)成為最小的方式，進行位置對準(zhǔn)，所 以能夠短時間地進行誤差分布彼此的精密的位置對準(zhǔn)。因此，通過根據(jù)本發(fā)明的來自多個視點的數(shù)據(jù)的綜合處理，能夠 在防止誤差的累積的同時，取得高精度的形狀。此外，本發(fā)明的數(shù)據(jù)結(jié)構(gòu)，由于是將體素結(jié)構(gòu)擴展了的結(jié)構(gòu)，所 以數(shù)據(jù)尺寸能夠比點群小而完成。即，由于在本發(fā)明提出的數(shù)據(jù)結(jié)構(gòu) 是在體素內(nèi)保存一個代表點，由于能夠使探測對應(yīng)于計測點的模型點的計算量級(order)為1,所以能夠?qū)⒄w的計算量級減少為O(N)。因此，在進行周圍環(huán)境和計測數(shù)據(jù)的位置對準(zhǔn)(ICP算法)時，由 于成為探測對象的數(shù)據(jù)量減少，所以能夠計算高效化。進而，原來的ICP算法對于稀疏數(shù)據(jù)輸出錯誤的結(jié)果，但本發(fā)明 的環(huán)境模型由于在體素內(nèi)具有代表點和誤差分布，所以對應(yīng)于稀疏數(shù) 據(jù)的位置對準(zhǔn)是可能的。此外，通過根據(jù)當(dāng)前的自身位置和誤差分布以及辨認了的自身位 置和誤差分布，并利用卡爾曼濾波器，校正自身位置和誤差分布，從 而能進一步提高自身位置的精度。因此，根據(jù)本發(fā)明的方法和裝置，具有將包含誤差的數(shù)據(jù)校正為 正確的信息的功能，并且對長時間的計測收斂到高精度。而且，位置 辨認處理由于是將體素的對應(yīng)于各體素的代表點及其誤差分布以計測 點來更新的處理，所以計算量小，運算不對周圍體素施加影響而封閉 在體素內(nèi)。因此，所以能夠高速處理。此外，計測數(shù)據(jù)能夠逐次綜合 到體素，其結(jié)果是得到的周圍環(huán)境信息的存儲器大小不會超過固定大小。進而，在所述模型更新步驟中，根據(jù)新輸入的被計測點的坐標(biāo)值 及其誤差分布、和已經(jīng)設(shè)定了的體素內(nèi)的代表點及其誤差分布，通過 卡爾曼濾波器取得新的代表點和誤差分布并進行再設(shè)定，所以能夠得 到更接近真值的形狀。特別是，通過反復(fù)使用了卡爾曼濾波器的模型更新步驟，即使是 包含誤差的數(shù)據(jù)也能通過卡爾曼濾波器的效果得到收斂到真值的高精 度的形狀。此外，通過在所述精密對準(zhǔn)步驟中，代替以與所述鄰近的誤差分 布間距離有關(guān)的評價值(例如該距離的總和)成為最小的方式進行位 置對準(zhǔn)，而是以與一致度有關(guān)的評價值(例如，該一致度的總乘積) 成為最大的方式，對于針對先前的計測位置的環(huán)境模型，使新的計測 數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，或者使針對新的計測位置的環(huán)境模型 旋轉(zhuǎn)及平移，從而進行位置對準(zhǔn)，其中該一致度是通過基于所述鄰近 的誤差分布間的極大似然推定值而確定的，因此能夠考慮環(huán)境模型、 計測數(shù)據(jù)雙方的誤差進行位置對準(zhǔn)。此外，由于在所述輸出步驟中，在將所述體素的代表點的位置作 為三維形狀的計測值輸出到輸出裝置中時，由于將表示該計測值的可 靠性或精度的指標(biāo)，對該體素內(nèi)部的誤差分布的大小輸出裝置輸出， 所以在使用計測裝置時，對應(yīng)于應(yīng)用的內(nèi)容使用者能夠取舍選擇可靠 性低的計測值。此外，在所述輸出步驟中，在將所述體素的代表點的位置作為三 維形狀的計測值輸出到輸出裝置中時，在該體素內(nèi)部的誤差分布大小 比規(guī)定的基準(zhǔn)值大的情況下，該計測值的可靠性或精度定為比規(guī)定的 基準(zhǔn)低，不輸出到輸出裝置中該體素的所述計測值，由此，在使用計 測裝置時，由于能夠最初就僅使用可靠性高的計測值，所以能夠削減 處理的數(shù)據(jù)量，帶來可靠性的提高。此外，在所述數(shù)據(jù)輸入步驟中，使用距離傳感器，將三維形狀上 的坐標(biāo)值作為以任意的計測位置為原點的距離數(shù)據(jù)， 一 邊移動原點一 邊依次取得，特別是通過從不同方向得到距離數(shù)據(jù)，能夠綜合分布形 狀不同的誤差分布的距離數(shù)據(jù)，能夠提高精度。此外，在所述匹配步驟之后具有更新所述環(huán)境模型的模型更新步驟，在該才莫型更新步驟中，探測對應(yīng)于新輸入的被計測點的坐標(biāo)值的 體素，僅在所述體素內(nèi)的代表點以及誤差分布的至少任一個被新設(shè)定、 或被再設(shè)定、或?qū)⒃擉w素進一步分割并分級地分割為多個體素的情況 下，在所述輸出步驟中，通過使該體素的代表點的位置、誤差分布以 及體素的位置的至少任一 個作為三維形狀的計測值輸出到輸出裝置 中，從而輸出通過從距離傳感器新得到的被計測點受到影響的體素內(nèi) 的代表點等的值。因此，設(shè)想使用者與以往同樣的運用，以距離傳感 器的得到的原本的計測值就像置換為更高精度的計測值那樣，變得能 夠利用。這樣，能夠進行精度更高的三維形狀計測。此外，在所述輸出步驟中，將從距離傳感器的位置出發(fā)距離傳感 器可以計測的位置范圍的環(huán)境模型內(nèi)的體素代表點的位置，作為三維 形狀的計測值輸出到輸出裝置中，由此，即使在現(xiàn)有的距離傳感器的 計測值的分辨能力粗糙的情況下，也能像精度優(yōu)良且分辨能力高的距 離傳感器那樣使用，這樣，精度更高的三維形狀計測成為可能。本發(fā)明的其他目的以及有利的特征可從參照附圖的以下說明而明確。


圖1是專利文獻l的"自身位置辨認裝置"的示意圖。圖2是本發(fā)明的自身位置辨認裝置的整體結(jié)構(gòu)圖。 圖3是非專利文獻2公開的三維激光雷達的結(jié)構(gòu)圖 圖4A是表示在距離傳感器計測的極坐標(biāo)數(shù)據(jù)和誤差的關(guān)系的圖。 圖4B是表示將誤差分布作為包含在長方體中的橢圓體而近似的情 況的圖。圖5是用于執(zhí)行本發(fā)明的方法的裝置結(jié)構(gòu)圖。 圖6是表示本發(fā)明的方法的流程圖。 圖7是模型構(gòu)筑步驟的示意圖。 圖8是構(gòu)筑的環(huán)境模型的示意圖。圖9A是表示本發(fā)明的體素數(shù)據(jù)的數(shù)據(jù)結(jié)構(gòu)的圖，表示各體素數(shù)據(jù) 的存儲器布局例。圖9B是表示本發(fā)明的體素數(shù)據(jù)的數(shù)據(jù)結(jié)構(gòu)的圖，表示等級(level) 2(1， 1， 0)的體素具有代表點的情況的例子。圖10是粗略對準(zhǔn)步驟S6和精密對準(zhǔn)步驟S7的數(shù)據(jù)處理流程圖。圖11是粗略對準(zhǔn)步驟S6的示意圖。圖12是精密對準(zhǔn)步驟S7的示意圖。圖13是模型更新步驟中的數(shù)據(jù)處理流程圖。圖14在相應(yīng)的體素內(nèi)有已經(jīng)設(shè)定了的代表點情況的示意圖。圖15表示在誤差分布相互重疊的情況下，根據(jù)兩誤差分布再設(shè)定 新的誤差分布和新的誤差分布的中心，其結(jié)果是新的代表點移動到其 他體素內(nèi)的情況。圖16是誤差分布相互重疊的情況的示意圖。圖17是表示通過綜合以多個計測位置為原點的距離數(shù)據(jù)從而代表 點的誤差分布縮小、代表點的精度提高的樣子的示意18是表示通過使用了卡爾曼濾波器的模型更新步驟得到的結(jié)果。圖19是圖18的局部放大圖。 圖20表示考慮了誤差的對應(yīng)。
具體實施方式
以下參照附圖對本發(fā)明的優(yōu)選實施方式進行說明。再有，在各圖 中，對共通的部分賦予同一附圖標(biāo)記，省略重復(fù)的說明。圖2是本發(fā)明的自身位置辨認裝置的整體結(jié)構(gòu)圖。在該圖中，本 發(fā)明的自身位置辨認裝置是六自由度的辨認單元，是從距離傳感器、 測距器、照相機、GPS、姿勢傳感器引入外界的三維信息來辨認自身位 置的裝置。再有，在本發(fā)明中，自身位置意味著自身位置辨認裝置本 身在外界的六自由度的位置和姿勢。再有，在本發(fā)明中距離傳感器以外的測距器、照相機、GPS、姿勢 傳感器不是不可缺的，而是對應(yīng)于需要作為選項而使用。以下，說明 使用距離傳感器的例子。圖3是作為距離傳感器的一個例子的三維激光雷達的結(jié)構(gòu)圖。例 如非專利文獻3公開的三維激光雷達。如該圖所示，三維激光雷達10包括雷達頭(radar head) 12和控 制器20。從激光二極管13激發(fā)的脈沖激光1在投射透鏡14被整形為 平行光2，用反射鏡18a、 18b和旋轉(zhuǎn)/搖動的多角鏡15在二維方向上進行掃描，照射到測定對象物。從測定對象物反射的脈沖激光3，經(jīng)由多角鏡15在受光透鏡16會聚，在光檢測器17轉(zhuǎn)換為電信號?？刂破?0內(nèi)的時間間隔計數(shù)器21,計測與激光二極管13的脈沖 振蕩定時同步的起動脈沖4、和從光檢測器17輸出的停止脈沖5的時 間間隔。信號處理板22將檢測出反射光時刻的時間間隔t、多角鏡的 旋轉(zhuǎn)角度e、搖動角度cp作為極坐標(biāo)數(shù)據(jù)(r, e, (p)輸出。r是以計測位置(雷達頭設(shè)置位置)為原點的距離，以r=cxt/2的 式子求出。在這里c是光速。判定處理單元23將來自信號處理板的極坐標(biāo)數(shù)據(jù)，向以雷達頭設(shè) 置位置為原點的三維空間數(shù)據(jù)(x, y, z)轉(zhuǎn)換，進行檢測處理。再有 在該圖中24是驅(qū)動單元。所述三維激光雷達10的計測范圍，例如是水平視場角60°、垂直 視場角30。、最大測定距離50m。此外，位置檢測精度例如是大約20cm。此外，在將計測數(shù)據(jù)以對各像素具有進深方向的距離值的距離圖 像來表示的情況下， 一幀的計測點數(shù)為橫向166點，掃描方向是50點 時，1幀表示166x50=8300點。這種情況下幀頻例如大約是2幀/秒。在該三維激光雷達10計測的三維形狀上的計測點，是橫向上Aexr、 垂直方向上Acpxr、互相離散的點群。例如，在Ae二60/166x兀/18(^ 6.3xl(T3 弧度、Aq^30/50x兀/180-10.5xlO-3弧度、產(chǎn)50m的情況下，即使是最接 近的情況，被計測點的間隔也是在橫向大約是315mm,在垂直方向大 約是525mm。在本發(fā)明，作為距離傳感器，例如使用所述的三維激光雷達10。 但是，距離傳感器并不限定于此，也可以使用利用視差的距離傳感器 等其他公知的距離傳感器。圖4A、圖4B是表示在距離傳感器計測的極坐標(biāo)數(shù)據(jù)和誤差的關(guān) 系的圖。如圖4A所示，將以任意的計測位置為原點的極坐標(biāo)值(r, e, (p) 作為計測結(jié)果進行計測。根據(jù)距離傳感器的計測結(jié)果中，通常存在圖 示那樣的誤差分布。該誤差分布在誤差分布的在rs、 es 、 cps的存在概率為P(rs， es, cps) 的情況下，誤差分布是在計測的軸r、 9、 (f)方向上正態(tài)分布，例如能夠 以式(1)表示。這里，r、 0、 cp是來自傳感器的計測值，(Jr、 ae、 (59是標(biāo)準(zhǔn)偏差，A是歸一化常數(shù)。如圖4B所示，誤差分布是通常內(nèi)包于r方向上的長切頭圓錐形(左 圖)的分布，但在遠方a和b的差小。因此，能夠?qū)⒃撜`差分布作為 包含在長方體中的橢圓體而近似于安全側(cè)。[算式2]<formula>formula see original document page 29</formula>圖5是用于執(zhí)行本發(fā)明的方法的裝置結(jié)構(gòu)圖。如該圖所示，該裝 置具備數(shù)據(jù)輸入裝置32、外部存儲裝置33、內(nèi)部存儲裝置34、中央 處理裝置35以及輸出裝置36。數(shù)據(jù)輸入裝置32具有上述的距離傳感器，對存儲裝置輸入三維形 狀上的坐標(biāo)值。此外，例如并用測角儀、測距器(odometer)等，還輸 入距離傳感器的位置/姿勢或移動距離較好。再有，數(shù)據(jù)輸入裝置32還 具有鍵盤等通常的輸入單元較好。外部存儲裝置33是硬盤、軟(fl叩py (注冊商標(biāo)))盤、磁帶、高 密度磁盤等。外部存儲裝置33在環(huán)境模型的大小較大，不能保持輸入 到后述的內(nèi)部存儲裝置34的三維形狀上的坐標(biāo)值、體素位置以及代表 點及其誤差分布的整體的情況下，存儲相對于環(huán)境模型的一部分范圍 或整體范圍的輸入的三維形狀上的坐標(biāo)值、體素位置以及代表點及其 誤差分布的一部分或整體，而且存儲用于運行本發(fā)明的方法的程序。內(nèi)部存儲裝置34例如是RAM、 ROM等，保管相對于環(huán)境模型的 一部分范圍或整體范圍的、輸入的三維形狀上的坐標(biāo)值、體素位置以 及代表點及其誤差分布的一部分或整體，而且保管運算信息。中央處理裝置35 (CPU)作為模型構(gòu)筑裝置、匹配裝置、粗略對 準(zhǔn)和精密對準(zhǔn)的位置對準(zhǔn)裝置、模型更新裝置、數(shù)據(jù)傳輸裝置而發(fā)揮 功能，集中地處理運算和輸入輸出等，與內(nèi)部存儲裝置34—起執(zhí)行程 序。模型構(gòu)筑裝置是進行后述的模型構(gòu)筑步驟的裝置，匹配裝置是進 行后述的匹配步驟的裝置，位置對準(zhǔn)裝置是進行后述的粗略對準(zhǔn)步驟 和精密對準(zhǔn)步驟的裝置，模型更新裝置是進行后述的模型更新步驟的裝置，數(shù)據(jù)傳輸裝置是向輸出裝置36輸出數(shù)據(jù)的裝置。輸出裝置36例如是顯示裝置、打印機、外部存儲裝置等，對內(nèi)部 存儲裝置34以及外部存儲裝置33的至少任一個輸出存儲的數(shù)據(jù)以及 程序的執(zhí)行結(jié)果。與外部裝置的接口是LAN、 USB、 IEEE1394等，對 應(yīng)于要求而輸出對輸入的三維形狀上的坐標(biāo)值附加了相應(yīng)的體素內(nèi)的 代表點、誤差分布、體素位置等的結(jié)果，或環(huán)境模型整體或環(huán)境模型 的一部分。上述本發(fā)明的裝置是將所述距離傳感器和通常的PC (計算機)組 合在一起的也可，或是將整體一體化的裝置也可。此外， 一體地裝入 能夠自動推進的裝置內(nèi)也可圖6是表示本發(fā)明的方法的流程圖。本發(fā)明的方法是引入外界的三維形狀來辨認自身位置的自身位置 辨認方法以及三維形狀的計測方法，具有數(shù)據(jù)輸入步驟S1、數(shù)據(jù)校 正步驟S2、探測范圍限定步驟S3、模型構(gòu)筑步驟S4、匹配步驟S5、 自身位置辨認步驟S7、 SIO、粗略對準(zhǔn)步驟S8、精密對準(zhǔn)步驟S9、沖莫 型更新步驟Sll以及輸出步驟S12。再有，在該一系列的處理中，Sl、 S2、 S3、 S5~S12每當(dāng)?shù)玫接?測數(shù)據(jù)時實施，S4僅在初次得到計測數(shù)據(jù)時實施。在數(shù)據(jù)輸入步驟Sl,在新的計測位置處，使用距離傳感器，對計 算機的存儲裝置輸入三維形狀上的坐標(biāo)值。此外，例如并用測角儀、 測距器等還輸入距離傳感器的位置/姿勢或移動距離較好。再有，在該數(shù)據(jù)輸入步驟S1中，使用三維激光雷達IO,將三維形 狀上的坐標(biāo)值作為以任意的計測位置為原點的距離數(shù)據(jù)， 一 邊移動原 點一邊依次取得較好。在作為距離傳感器使用三維激光雷達10的情況下，三維形狀上的 坐標(biāo)值是以任意的計測位置為原點的距離數(shù)據(jù)，以極坐標(biāo)值(r, e, (p) 表示。此外，各坐標(biāo)值的誤差分布根據(jù)極坐標(biāo)值(r, e, (p)以運算求 得，或預(yù)先以別的輸入單元(例如鍵盤)輸入。在數(shù)據(jù)校正步驟S2,進行距離數(shù)據(jù)的校正處理，提高距離數(shù)據(jù)的 精度。此外，根據(jù)極坐標(biāo)數(shù)據(jù)和測距器的數(shù)據(jù)，轉(zhuǎn)換為以任意的固定 位置為原點的三維空間數(shù)據(jù)(x， y, z)也可。在距離數(shù)據(jù)的校正處理，進行孤立點的去除、統(tǒng)計上的處理等。孤立點是從周圍的點孤立而存在的點，由于計測數(shù)據(jù)是以多個接近的 點構(gòu)成的，所以能夠假定孤立點是錯誤計測而去除。統(tǒng)計上的處理是 考慮計測數(shù)據(jù)包含的誤差分布，通過統(tǒng)計處理(例如平均值等)多次 的計測，進行距離的校正。進而，成為對象的三維形狀在能夠直線近似或平面近似的情況下 進4t這些也可。在探測范圍限定步驟S3,限定距離傳感器的探測范圍。 當(dāng)不限定探測范圍而對環(huán)境模型進行計測數(shù)據(jù)的匹配處理時，有 得到多個解(被計測點)的可能性。因此，實施(1)根據(jù)過去的傳 感器位置的變化推定當(dāng)前的傳感器位置，探測傳感器位置推定結(jié)果的 附近，(2)使用測距器推定傳感器位置，限定探測范圍，(3)距離數(shù) 據(jù)中，不僅是距離值，也利用反射強度值來縮小探測結(jié)果等等。圖7是對體素的分割使用八叉樹的情況下的模型構(gòu)筑步驟的示意圖。在模型構(gòu)筑步驟S4，如該圖所示那樣，構(gòu)筑環(huán)境模型，該環(huán)境模 型是將三維形狀所存在的區(qū)域，分割為由分界表面相互正交的長方體 構(gòu)成的多個體素6，存儲各體素位置。體素6的形狀是各邊的長度相等的立方體，或各邊的長度不同的 長方體均可。此外，體素6的各邊的長度，將最大的體素6設(shè)定為相當(dāng)于必要 最小限度的分辨能力的大小較好。以下，將最大的體素6稱為等級1 的體素。此外，在單一的體素內(nèi)存在多個被計測點的情況下，以單一的體 素內(nèi)僅存在單一的被計測點的方式，例如在選擇八叉樹的情況下，進 一步八分割體素，分級地分割為多個體素。以下，將實施了一次最大 的體素6的八分割的空間區(qū)域稱為等級2的體素，將實施了 k次的空 間區(qū)域稱為等級k+l的體素。圖8是構(gòu)筑的環(huán)境模型的示意圖。在匹配步驟S5，如該圖所示，在對應(yīng)于三維形狀上的坐標(biāo)值的體 素6內(nèi)部設(shè)定并存儲代表點7與其誤差分布8。末端的體素能夠僅具有 一個計測值的代表點。各體素通過具有計測值的代表點及其誤差分布， 從而表示物體的形狀。此外，也能使體素具有表示物體存在概率的概率值。在匹配步驟S5 ，通過[算式3]的式(2)給出代表點的絕對位置。這里， (x, y, z)是代表點在體素的相對坐標(biāo)，Sx、 Sy、 Sz是在等級1的體 素的一邊的大小，nx(k)、 ny(k)、 nz(k)是在等級k的體素的地址號碼，L 是所要求的代表點存在的等級。[算式3]<formula>formula see original document page 32</formula>圖9A、圖9B是表示本發(fā)明的體素數(shù)據(jù)的數(shù)據(jù)結(jié)構(gòu)的圖。在該圖中，圖9A是各體素數(shù)據(jù)的存儲器布局的例子。在該圖中， 箭頭表示對數(shù)據(jù)的鏈接，將對數(shù)據(jù)的指針作為值而保持。圖9B表示等級2(1, 1, 0)的體素具有代表點的情況的例子。再 有在該圖中，null表示空集合。所述數(shù)據(jù)結(jié)構(gòu)的環(huán)境模型，具有以下特征。(1) 內(nèi)容以小長方體分割空間將計測點的代表點和誤差分布保持到 各體素(2) 精度相當(dāng)于每個體素具有的計測點的代表值。(3) 存在能夠表現(xiàn)物體存在的有無。(4) 數(shù)據(jù)量與體素的個數(shù)成比例地需要存儲器，但大小是固定的。(5) 來自點群的轉(zhuǎn)換適應(yīng)，計算量少。(6) 訪問速度由于是簡單的結(jié)構(gòu)，所以對要素的訪問是高速的。 此外根據(jù)該特征，所述環(huán)境模型滿足以下全部效果A ~ C。效果A:考慮了誤差的表現(xiàn)是可能的。 效果B:需要的存儲量和計算量在一定量以下。 效果C:不僅能表示物體的存在，也能表示不存在物體。在圖6中，在匹配步驟S5之后，在步驟S6片企查在先前的計測位 置的同 一 三維形狀的數(shù)據(jù)的有無。該檢測在先前的計測的同 一 三維形 狀數(shù)據(jù)不存在的情況下，將新的計測位置辨認為自身位置。該辨認在逐次移動的移動體的初始位置中，優(yōu)選以已知的六自由度位置(例如全局坐標(biāo)系(global coordinate)的原點)實施。此外， 在該辨認中，辨認包含新的計測位置的位置(三自由度)和姿勢(三 自由度)的六自由度位置較好。此外，在步驟S6的檢查，在先前的計測位置的同一三維形狀的數(shù) 據(jù)存在的情況下，在圖6中，在匹配步驟S5之后實施粗略對準(zhǔn)步驟S8 和精密對準(zhǔn)步驟S9。圖10是粗略對準(zhǔn)步驟S8和精密對準(zhǔn)步驟S9的數(shù)據(jù)處理流程圖， 圖11是粗略對準(zhǔn)步驟S8的示意圖，圖12是精密對準(zhǔn)步驟S9的示意 圖。在圖10中，在粗略對準(zhǔn)步驟S8，如圖11所示，對于針對先前的 計測位置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，并 以與具有代表點的體素間距離有關(guān)的評價值(例如，該距離的總和) 成為最小的方式，進行位置對準(zhǔn)，其中該代表點是與計測數(shù)據(jù)以及誤 差分布鄰近的代表點，或者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平 移，并以與具有代表點的體素間距離有關(guān)的評價值(例如，該距離的 總和)成為最小的方式，進行位置對準(zhǔn)，或者，使新的計測數(shù)據(jù)以及 誤差分布旋轉(zhuǎn)及平移，并以與具有代表點的體素的概率值有關(guān)的評價 值(例如，該概率值的總和)成為最大的方式，進行位置對準(zhǔn)，其中 該代表點是與計測數(shù)據(jù)以及誤差分布鄰近的代表點，或者使針對新的 計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與鄰近的體素所具有的概率值 的差有關(guān)的評價值(例如，該概率值的差的總和)成為最小的方式， 進行位置對準(zhǔn)。粗略對準(zhǔn)步驟S6中的位置對準(zhǔn)，是通過在體素空間上表現(xiàn)環(huán)境模 型和計測數(shù)據(jù)，或環(huán)境模型在體素空間上、計測數(shù)據(jù)表現(xiàn)代表點和誤 差分布，從而實施的。設(shè)當(dāng)前的計測數(shù)據(jù)是在位置(x， y， z)、姿勢 (e， cp, v)的計測，將計測數(shù)據(jù)轉(zhuǎn)換為世界坐標(biāo)來計算與環(huán)境模型的 一致度。一致度的計算例如能夠使用最短距離法。在使用最短距離法的情 況下的體素間距離，當(dāng)兩個體素空間為x(1)、 x(2),體素的總數(shù)為I，體 素的值為x"時，能夠以[算式4]的式(3)定義。計測數(shù)據(jù)的最合適的位置/姿勢，能夠利用通過使位置(x, y, z)、姿勢(e, cp, 變化而使s為最小的最小二乘法來計算。此外，作為一致度，例如在環(huán)境模型和計測數(shù)據(jù)的兩體素中，能 夠使用與鄰近的兩體素的具有的概率值的差有關(guān)的評價值(例如，該 概率值的差的總和)。在這種情況下以一致度為最小的方式，使計測數(shù) 據(jù)的最適合的位置/姿勢變化。此外，在環(huán)境模型在體素空間上、計測數(shù)據(jù)表現(xiàn)代表值和誤差分 布的情況下，能夠使用與計測數(shù)據(jù)的代表值以及誤差分布鄰近的環(huán)境 模型的體素的概率值有關(guān)的評價值(例如，該概率值的總和)。在該情 況下以 一 致度為最大的方式，使計測數(shù)據(jù)的最合適的位置/姿勢變化。[算式4]<formula>formula see original document page 34</formula>…(3)在圖10中，在精密對準(zhǔn)步驟S9，如圖12所示那樣，對于針對先 前的計測位置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移， 或者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與鄰近的誤差 分布間距離有關(guān)的評價值(例如，該距離的總和)成為最小的方式， 進行位置對準(zhǔn)。在精密對準(zhǔn)步驟S7中的環(huán)境模型和計測數(shù)據(jù)的位置對準(zhǔn)中，利用 對能夠使點群和點群的位置對準(zhǔn)的ICP算法考慮了誤差分布的手法。 位置對準(zhǔn)的初始值利用通過粗略位置對準(zhǔn)所得到的位置/姿勢。對ICP算法利用的誤差分布間的距離的計算，例如將誤差分布交 叉的情況考慮為同一計測點，將根據(jù)分布的一致度求得的加權(quán)(weight) 乘以該情況的距離值來計算。分布的 一致例如能夠使用馬哈拉諾比斯 距離那樣的距離尺度。該情況的環(huán)境模型和計測數(shù)據(jù)的距離，當(dāng)環(huán)境模型數(shù)據(jù)為PMi,環(huán)境模型數(shù)據(jù)的誤差分布為SMl，計測數(shù)據(jù)為PDl,計測數(shù)據(jù)的誤差分布 為SrM,誤差分布的合成函數(shù)為w,對應(yīng)于計測數(shù)據(jù)的環(huán)境模型數(shù)據(jù)的 個數(shù)為N時，能以[算式5]的式(4)定義。這里，T表示轉(zhuǎn)置。計測數(shù)據(jù)的最合適的位置/姿勢，能夠利用通過使計測了計測數(shù)據(jù)的位置(x, y, z)、姿勢(e, (p, \|/)變化并移動Pm而使s為最小的 最小二乘法來計算。此外，辨認自身位置/姿勢的同時，辨認自身位置的誤差分布，根 據(jù)當(dāng)前的自身位置和誤差分布以及辨認了的自身位置和誤差分布，利 用卡爾曼濾波器校正自身位置和誤差分布。[算式5]<formula>formula see original document page 35</formula>進而在圖6中，模型更新步驟Sll在自身位置辨認步驟S10之后 實施，更新在模型構(gòu)筑步驟S4構(gòu)筑的環(huán)境模型。圖13是模型更新步驟S11中的數(shù)據(jù)處理流程圖。如該圖所示，在 步驟ST1探測對應(yīng)于新輸入的被計測點的坐標(biāo)值的體素，在步驟ST2 相應(yīng)體素內(nèi)沒有代表點(體素是空的)的情況下，將在步驟ST3新輸 入的^f皮計測點的坐標(biāo)值和誤差分布作為代表點的坐標(biāo)值和誤差分布而 設(shè)定(新注冊)。此外，在該步驟ST3中，新的計測位置(原點)和^皮計測點之間， 原理上物體不應(yīng)該存在。因此再設(shè)定或擦除位于新的計測位置(原點) 和被計測點之間的體素內(nèi)的代表點和誤差分布。圖14是在相應(yīng)體素內(nèi)有已經(jīng)設(shè)定了的代表點的情況的示意圖。在圖13的步驟ST2相應(yīng)體素內(nèi)有已經(jīng)設(shè)定了的代表點的情況下， 在步驟ST4比較新取得的誤差分布和已經(jīng)設(shè)定了的體素內(nèi)的誤差分布 (即判斷是不同點還是相同點)。在該比較，在誤差分布相互重疊的情況下(圖14的(A)),在步 驟ST5根據(jù)兩誤差分布、或根據(jù)兩誤差分布和體素內(nèi)已經(jīng)設(shè)定了的代 表點和新輸入的被計測點的坐標(biāo)值，再設(shè)定新的誤差分布和新的代表 點(即合成誤差分布)。此外在該比較，在誤差分布不相互重疊的情況下(圖14的(B)), 在步驟ST6、 ST7以在單一的體素內(nèi)僅存在單一的代表點的方式，進一 步八分割該體素并分級地分割為多個體素并新注冊。分割和合成的基準(zhǔn)，例如根據(jù)誤差分布的一致度來判斷。誤差分布的 一致度例如能夠利用馬哈拉諾比斯距離那樣的距離尺度。此外， 基于兩個誤差分布，通過統(tǒng)計上的檢驗來判定兩者是不是表示同一點 也可。在步驟ST5根據(jù)兩誤差分布再設(shè)定新的誤差分布和新的誤差分布 的中心，其結(jié)果是新的代表點向其他體素內(nèi)移動時(即，在步驟ST8， "是")，返回步驟ST2，反復(fù)進行上述處理。再有，圖15表示在步驟ST5根據(jù)兩誤差分布、或根據(jù)兩誤差分 布和體素內(nèi)已經(jīng)設(shè)定了的代表點和新輸入的被計測點的坐標(biāo)值，再設(shè) 定新的誤差分布和新的誤差分布的中心，其結(jié)果是新的代表點向其他 體素內(nèi)移動的情況。在設(shè)定表示體素內(nèi)物體的存在概率的概率值的情況下，在模型更 新步驟Sll中，對應(yīng)于體素內(nèi)的代表點和誤差分布的新注冊、或再設(shè) 定、或擦除、或在分割后的新注冊的處理，體素內(nèi)的概率值也通過統(tǒng) 計處理，進行新注冊、或再設(shè)定、或擦除、或在分割后的新注冊。圖16是誤差分布相互重疊的情況(圖14的(A))的其他的示意 圖。在步驟ST5中，作為合成兩個代表點和誤差分布并設(shè)定新的代表 點和誤差分布的單元，能夠使用卡爾曼濾波器。例如在二維的情況下， 如該圖所示那樣，當(dāng)兩個代表點分別是x(l)、 x(2)，兩個誤差分布分別 是2(1)、 S'(2),將其合成的代表點是x(2)、誤差分布是S(2)時，計算 代表點x(2)和誤差分布2:(2)的示意圖如圖16。在圖6中，在輸出步驟S12,將所述自身位置、基于所述自身位置 的所述體素位置、代表點以及誤差分布的至少任一個對輸出裝置36輸 出。輸出裝置36是顯示裝置(例如CRT)的情況下，優(yōu)選在三維圖像 上立體顯示自身位置。此外，將自身位置的數(shù)據(jù)轉(zhuǎn)送到其他的裝置(例 如控制裝置、計算機)也可，在打印機輸出也可。此外，在輸出步驟S12,將體素的代表點的位置作為三維形狀的計 測值對輸出裝置36輸出，并且將表示該計測值的可靠性或精度的指標(biāo) (例如，數(shù)值)，基于該體素內(nèi)部的誤差分布的大小，對輸出裝置36 輸出也可。進而，在輸出步驟S12中，將體素的代表點的位置作為三 維形狀的計測值對輸出裝置36輸出時，在該體素的內(nèi)部的誤差分布的 大小(擴展)比規(guī)定的基準(zhǔn)值大的情況下，該計測值的可靠性或精度 定為比規(guī)定的基準(zhǔn)低，該體素的所述計測值(即，該體素的代表點的位置)不對輸出裝置36輸出也可。此外，在所述匹配步驟之后具有更新所述環(huán)境模型的模型更新步 驟，在該模型更新步驟中，探測對應(yīng)于新輸入的被計測點的坐標(biāo)值的 體素，在所述體素內(nèi)的代表點以及誤差分布的至少任 一 個被新設(shè)定、 或被再設(shè)定、或?qū)⒃擉w素進一步分割并分級地分割為多個體素的情況 下，在所述輸出步驟中，將該體素的代表點的位置作為三維形狀的計 測值輸出到輸出裝置中。此外，在輸出步驟S6中，能夠得到距離傳感器的位置/姿勢的情況 下，將從該位置能看到的范圍的環(huán)境模型內(nèi)的體素的代表點位置作為 三維形狀的計測值輸出到輸出裝置中也可。從距離傳感器的位置能看 到的范圍，指的是從距離傳感器的位置出發(fā)距離傳感器能進行位置計 測的范圍，可包含從距離傳感器的位置出發(fā)距離傳感器能進行位置計 測的角度范圍(視野)、和從距離傳感器的位置出發(fā)距離傳感器能進行 位置計測的距離范圍。如圖6所示的處理的次序，在新的計測位置處，每當(dāng)?shù)玫叫碌挠?測數(shù)據(jù)時，反復(fù)進行處理，在內(nèi)部存儲裝置34以及外部存儲裝置33 的至少任一個中存放結(jié)果。為了使處理高速化，優(yōu)選在容量允許的限 度內(nèi)盡可能的將結(jié)果存放在內(nèi)部存儲裝置34。根據(jù)所述的本發(fā)明的方法和裝置，由于將三維形狀所存在的空間 區(qū)域分割為多個體素6,將各體素位置存儲在內(nèi)部存儲器34以及外部 存儲裝置33的至少任一個中，所以即使在計測對象物大的情況下，也 能將數(shù)據(jù)量抑制為與體素數(shù)成比例的小的數(shù)據(jù)尺寸。此外，由于在對應(yīng)于坐標(biāo)值的體素6的內(nèi)部設(shè)定并存儲代表點7 及其誤差分布8,所以能夠表現(xiàn)體素的分辨能力以上的信息。因此，根據(jù)本發(fā)明的數(shù)據(jù)結(jié)構(gòu)，能夠?qū)⒍鄠€視點的計測數(shù)據(jù)綜合 為固定大小。概率值，從而即使在誤差分布比代表點所屬的體素擴展的情況下，也 不用找到代表點所屬的體素并從其誤差分布再計算各體素中的物體的 存在有無，而是僅以該體素的概率值就能容易判斷，所以能夠抑制檢 索時間。此外，由于根據(jù)過去的計測位置的變化推定當(dāng)前的計測位置，或通過能夠取得當(dāng)前的計測位置的傳感器來取得，或不僅利用計測數(shù)據(jù) 的距離值還利用反射強度值，限定核對的范圍，所以能夠抑制檢索時 間。此外，由于在粗略對準(zhǔn)步驟S6中，對于針對先前的計測位置的環(huán) 境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，并以與具有代表 點的體素間距離有關(guān)的評價值(例如，該距離的總和)成為最小的方 式，進行位置對準(zhǔn)，其中該代表點是與計測數(shù)據(jù)以及誤差分布鄰近的 代表點，或者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與具 有代表點的體素間距離有關(guān)的評價值(例如，該距離的總和)成為最 小的方式，進行位置對準(zhǔn)，或者，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn) 及平移，并以與具有代表點的體素的概率值有關(guān)的評價值(例如，該 概率值的總和)成為最大的方式，進行位置對準(zhǔn)，其中該代表點是與 計測數(shù)據(jù)以及誤差分布鄰近的代表點，或者使針對新的計效'J位置的環(huán) 境模型旋轉(zhuǎn)及平移，并以與鄰近的體素所具有的概率值的差有關(guān)的評 價值(例如，該概率值的差的總和)成為最小的方式，進行位置對準(zhǔn)， 所以能夠一邊防止誤差的累積， 一邊短時間地進行具有代表點的體素 彼此的位置對準(zhǔn)。此外，由于對于針對先前的計測位置的環(huán)境模型，使針對新的計 測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與鄰近的體素所具有的概率值的 差有關(guān)的評價值(例如，該概率值的差的總和)成為最小的方式，進 行位置對準(zhǔn)，在該情況下，還附加不存在物體的信息進行位置對準(zhǔn)， 所以能夠謀求精度的提高。接著，由于在精密對準(zhǔn)步驟S7中，對于針對先前的計測位置的環(huán) 境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，或者使針對新的 計領(lǐng)'M立置的環(huán)境模型旋轉(zhuǎn)及平移，并以與鄰近的誤差分布間距離有關(guān) 的評價值(例如，該距離的總和)成為最小的方式，進行位置對準(zhǔn)， 所以能夠短時間地進行誤差分布彼此的精密的位置對準(zhǔn)。因此，通過根據(jù)本發(fā)明的從多個視點的數(shù)據(jù)的綜合處理，能夠防 止誤差的累積的同時，取得高精度的形狀。此外，由于本發(fā)明的數(shù)據(jù)結(jié)構(gòu)是將體素結(jié)構(gòu)擴展了的結(jié)構(gòu)，所以 數(shù)據(jù)尺寸能夠比點群小而完成。因此，在進行周圍環(huán)境和計測數(shù)據(jù)的 位置對準(zhǔn)(I CP算法)時成為探測對象的數(shù)據(jù)量減少，能夠計算高效化。進而，原來的ICP算法對于稀疏數(shù)據(jù)輸出錯誤的結(jié)果，但本發(fā)明 的環(huán)境模型由于在體素內(nèi)具有代表點和誤差分布，所以對應(yīng)于稀疏數(shù) 據(jù)的位置對準(zhǔn)是可能的。此外，由于在模型構(gòu)筑步驟S4中，在將最大的體素9設(shè)定為相當(dāng) 于必要最小限度的分辨能力的大小，而且單一的體素9內(nèi)存在多個被 計測點的情況下，以在單 一 的體素內(nèi)僅存在單 一 的被計測點的方式， 進一步將該體素八分割并分級地分割成多個體素，所以能夠?qū)?shù)據(jù)量 抑制為小的數(shù)據(jù)尺寸的同時，能夠使用分割后的體素和代表點進一步 提高分辨率。特別是，將三維形狀上的多個坐標(biāo)值作為以多個計測位置為原點 的距離數(shù)據(jù)而取得，通過將該距離數(shù)據(jù)的坐標(biāo)值作為所述代表點的坐 標(biāo)值，將距離數(shù)據(jù)的坐標(biāo)值的計測誤差作為代表點的誤差分布，從而 能夠使用正確的坐標(biāo)值和誤差分布在統(tǒng)計上綜合多次的計測，能夠愈 發(fā)提高精度。圖17表示通過綜合以多個計測位置為原點的距離數(shù)據(jù)，從而代表 點的誤差分布縮小，代表點的精度提高的樣子。因為這樣地以不同的 計測位置(即作為距離傳感器的三維計測器的位置)為原點而得到的 距離數(shù)據(jù)其誤差分布的朝向也不同，所以通過經(jīng)由環(huán)境模型將這些距 離數(shù)據(jù)逐次綜合，從而代表點的誤差分布縮小，代表點的位置精度提 高。再有，在圖17中，三維計測后的圖是表示了杯子的二維截面的示 意圖，三維計測后的圖的虛線表示杯子的實際表面。此外，辨認自身位置的同時，辨認自身位置的誤差分布，根據(jù)當(dāng) 前的自身位置和誤差分布以及辨認了的自身位置和誤差分布，利用卡 爾曼濾波器校正自身位置和誤差分布，能夠愈發(fā)提高精度。此外，作為在原點和^皮計測點之間不存在物體的，通過再設(shè)定或 擦除位于其間的體素內(nèi)的代表點和誤差分布，從而能夠去除錯誤計測 數(shù)據(jù)的影響。此外，探測對應(yīng)于新輸入的被計測點的坐標(biāo)值的體素，在該體素 內(nèi)沒有代表點的情況下，通過將所述坐標(biāo)值和誤差分布設(shè)定為代表點 的坐標(biāo)值和誤差分布，從而能夠容易地設(shè)定代表點的坐標(biāo)值和誤差分布。進而，在所述體素內(nèi)有已經(jīng)設(shè)定了的代表點的情況下，比較新取得的誤差分布和已經(jīng)設(shè)定了的體素內(nèi)的誤差分布，在誤差分布相互重 疊的情況下，根據(jù)兩誤差分布、或根據(jù)兩誤差分布和體素內(nèi)已經(jīng)設(shè)定 了的代表點和新輸入的被計測點的坐標(biāo)值，再設(shè)定新的誤差分布和新 的代表點，在誤差分布不相互重疊的情況下，以在單一的體素內(nèi)僅存 在單一的代表點的方式，將該體素進一步八分割并分級地分割為多個 體素。由此，能夠回避誤差的累積并收斂到高精度的形狀。因此，根據(jù)本發(fā)明的方法和裝置，具有將包含誤差的距離數(shù)據(jù)校 正為正確的信息的功能，并且對于長時間的計測收斂到高精度的形狀。 而且，位置辨認處理由于是將體素的對應(yīng)于各體素的代表點及其誤差 分布以計測點更新的處理，所以計算量小，運算不對周圍的體素施加 影響而封閉在體素內(nèi)。因此，所以能夠高速處理。此外，計測數(shù)據(jù)能 夠逐次綜合到體素，其結(jié)果是得到的周圍環(huán)境信息的存儲器大小不會 超過固定大小。對使用了卡爾曼濾波器的模型更新步驟進行詳細說明。 在使用了卡爾曼濾波器的模型更新步驟的情況下，根據(jù)新輸入的 被計測點的坐標(biāo)值及其誤差分布、和已經(jīng)設(shè)定了的體素內(nèi)的代表點及 其誤差分布，通過卡爾曼濾波器取得新的代表點和誤差分布并進行再 設(shè)定。以各模型點群的位置m(i)為狀態(tài)量，基于距離傳感器的計測點的 位置，以下式[算式6]表現(xiàn)模型。再有，在本實施例，m(i)是體素內(nèi)部 的代表點(以下相同)。[算式6]L(j) = hm(Rr,t"m(i)) + vL(j)=Rs-MRr-1(111(0 - tr) - ts) + vL(j) (j = i，... ，N)在[算式6]中，L(j)是根據(jù)距離傳感器的計測位置。例如L(j)是在距 離傳感器的傳感器坐標(biāo)系中三維LRF(激光測距儀)的計測點j( j=1,..., N)的位置LCJ)=(xL(j), yL(j), ZL(j))t。這里，t表示轉(zhuǎn)置矩陣(以下相 同)。hm(Rr, tr, m(i))是對L(j)的觀測系模型。Rr是表示裝載了距離傳感器的移動體(例如移動機器人)的相對于世界坐標(biāo)系的姿勢的旋轉(zhuǎn)矩陣Rr=R(ex, ey， ez)。再有，ex、 ey、 ez分別表示繞x軸、y軸、z軸的旋轉(zhuǎn)角(以下相同)。tr是表示上述移動體的相對于世界坐標(biāo)系的位置的平移矢量t「(X，y, z)。ViXi)是加入到距離傳感器的計測值L(j)的觀測噪聲。 Rs是傳感器坐標(biāo)系的對移動體坐標(biāo)系的旋轉(zhuǎn)矩陣R^R (ex， ey，ez)。ts是表示傳感器坐標(biāo)系的對移動體坐標(biāo)系的位置的平移矢量tf(X，y， z)。測定對象物是靜止的，將測定對象物的位置t。姿勢Rr對環(huán)境模型固定。使根據(jù)距離傳感器的計測點群和環(huán)境模型點群上的點i(即代表點)相對應(yīng)。對進行該對應(yīng)的模型點群上的點i通過下式(4)進行更新。 再有，可僅對與根據(jù)距離傳感器的計測點群進行了對應(yīng)的模型點群上 的代表點m (i)通過如下的[算式7]進行更新。[算式7]Kmk(i)= 2mk,k—,卿JMHmk(j)2^,k—,卿』)"2Lk(j))-i m，k(i) = mk,w(i) +Kmk(i)(Lk(j) - lud^i^mkH, 。i卜W"i)-Kji)H，H(i)在[算式7]中，下標(biāo)k表示是在離散時刻k的值。關(guān)于mk(i)， m'k(i)表示mk(i)的更新值(事后推定值),mk, "i)表 示基于m'k-"i)的mk(i)的預(yù)測值(事前推定值)。再有因為環(huán)境(測定 對象物)靜止，所以mk, w(i卜m'w(i)。Smk(i)是體素內(nèi)部的代表點mk(i)的誤差協(xié)方差矩陣(即所述誤差分 布)。此外，關(guān)于i:mk(i)， f"i)表示Sd(i)的更新值(事后推定值),i:mk,W(l)表示基于S'nJ^(i)的2^k(i)的預(yù)測值(事前推定值)。將在傳感器坐標(biāo)系中將三維LRF的計測點j(j^， ...， N)的位置以L(j)表示，將其誤差協(xié)方差矩陣以&(0表示。在這里N是在三維LRF得到的計測點的總 數(shù)。作為三維LRF的誤差模型假定與計測距離沒有關(guān)系的一定的正態(tài) 分布。將對傳感器坐標(biāo)系的x軸方向照射激光的情況下的誤差協(xié)方差 矩陣作為Zs。對應(yīng)激光的照射方向，誤差分布也改變姿勢。SiXJ)是將 對基準(zhǔn)方向的激光照射方向使用旋轉(zhuǎn)矩陣RL(j)表示為 SL(j)=RL(j)SsRLt(j)。計測點j的在世界坐標(biāo)系的位置z(j)、及其誤差協(xié) 方差矩陣Sz(j)能夠分別以z(j)=Rr(RsL(j)+ts)+tr、 ^CJhRrRsSLCORstRr1來 表示。Kw(i)是對mk(i)的卡爾曼增益。hmk(Rrk, trk， mk， w(i))是對Lk(j)、 i二Pk(j)的觀測系模型。i:Pk(j)是對應(yīng)于計測點j的環(huán)境地圖(即，環(huán)境模型)上的點。Hmk是對U(j)、 i-pk(j)的觀測系模型的雅可比矩陣，以下面的[算式8]表示。[算式8]Hmk(J)3 hmk(IU，m"i)) 3 mk(i)mk(i>根據(jù)卡爾曼濾波器的更新過程，在得到了環(huán)境地圖的模型點群的 各點(體素的代表點)的位置和誤差協(xié)方差矩陣的更新值m'k(i)、 Smk(i) 的階段，以下面的次序進行環(huán)境模型的更新。(1) 將這些更新值m'k(i)、 2:'mk(i)作為新的代表點、誤差分布再設(shè)定。(2) 所述(1)的結(jié)果是，代表點的位置移動到別的體素內(nèi)的情況下， 當(dāng)移動目的地的體素沒有保持代表點時,使移動目的地的體素保持移 動后的代表點及其誤差協(xié)方差矩陣，從移動起始地的體素去除代表點 等。當(dāng)移動目的地的體素已經(jīng)保持有代表點時，在兩個代表點中，判 斷其兩誤差分布是否重疊(與所述ST4中的判斷相同)。之后的處理與 圖13的ST4以后的處理相同即可。(3) 關(guān)于沒有進行與模型點群上的代表點m(i)對應(yīng)的根據(jù)距離傳感器 的計測點，在包含該計測點的體素不具有代表點的情況下，將計測點 及其誤差分布作為該體素的代表點和誤差分布追加并保持。如果體素內(nèi)代表點已經(jīng)存在的情況下，以包含位于體素內(nèi)的沒有進行對應(yīng)的其 他的多個計測點，既存的代表點和各計測點被包含在完全不同體素的 方式，分割體素并在此基礎(chǔ)上使代表點等繼承到分割后的體素。通過反復(fù)所述的使用了卡爾曼濾波器的模型更新步驟，從而漸漸 地體素內(nèi)的誤差協(xié)方差矩陣(即誤差分布)的范圍變小，并且體素變 得容易分割。通過分割體素，還能夠表現(xiàn)初始體素的大小以下的變化。圖18表示通過使用了卡爾曼濾波器的模型更新步驟得到的結(jié)果。 圖19是圖18的局部放大圖。在這些圖中，以初始的體素的一個邊的 長度為100cm,允許6分割以下的再分割數(shù)。在對象存在的區(qū)域，反 復(fù)體素的再分割，其結(jié)果是精度良好地表現(xiàn)計測對象?？芍趯ο蟛?存在的區(qū)域不進行體素的再分割，能夠以所需的充分的數(shù)據(jù)量來表現(xiàn) 環(huán)境。此外，各體素內(nèi)的代表點的誤差分布也小，能夠以高精度表現(xiàn) 環(huán)境地圖。這樣，即使是包含誤差的數(shù)據(jù)，也能夠通過卡爾曼濾波器 的效果，得到收斂到真值的結(jié)果。進而，在該方法通過使計測數(shù)據(jù)數(shù) 增加，從而標(biāo)準(zhǔn)偏差變小，能夠期待精度的進一步提高。此外，由于測定對象物的位置/姿勢固定，所以能夠與測定對象物 的位置/姿勢獨立來進行更新。再有，通過僅對與根據(jù)距離傳感器的計 測點群進行了對應(yīng)的模型點群上的代表點m(i),進行根據(jù)所述的卡爾 曼濾波器的更新，從而能夠大幅削減計算成本。在所述精密對準(zhǔn)(aligning)步驟中，也可代替以所述鄰近的誤差 分布間距離的總和為最小的方式進行位置對準(zhǔn)，而是以與 一致度有關(guān) 的評價值(例如，該一致度的總乘積)成為最大的方式，對于針對先 前的計測位置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移， 或者使針對新的計測位置的環(huán)境^t型旋轉(zhuǎn)及平移，從而進行位置對準(zhǔn)， 其中該 一致度是通過基于所述鄰近的誤差分布間的極大似然推定值而 確定的。對該情況進行詳細地說明。由于對作為環(huán)境地圖(環(huán)境模型)的模型點群、以及傳感器的計 測點群雙方考慮了誤差模型，所以對所述與一致度有關(guān)的評價值(例 如該一致度的總乘積)的計算式(以下叫做評價函數(shù))也能夠引入雙 方的誤差。在本實施例的情況下，不是單使最近的點對應(yīng)，而是通過 引入似然的概念，從而對評價函數(shù)附加環(huán)境地圖上的誤差分布、以及計測點的誤差分布，在當(dāng)前時刻的環(huán)境地圖中，確定在各計測點的出 現(xiàn)概率成為最大的情況下評價函數(shù)也成為最大。
具體地，假定為與計測點j對應(yīng)的環(huán)境地圖上的點i二p(j)的位置，
符合平均值(代表點)m(i)、誤差協(xié)方差矩陣Zm(i)的正態(tài)分布，在此
基礎(chǔ)上在三維LRF計測的結(jié)果是，將得到L(j)的計測數(shù)據(jù)的概率值Pr (L(j)lm(i)， Sm(i))作為點i和點j的評價函數(shù)EM(i， j),以其總乘積 為最大的方式以下面的[算式9]來確定評價函數(shù)。 [算式9]
J]^(力Pr(L(;') I m(尸(,/))，i:所(尸C/))"
其中，O)(j)在模型點群中有與計測點j對應(yīng)的點存在的情況下為1，
在除此之外的情況下為0。
這里，將Pr(L(j)lq)作為表示在環(huán)境地圖的點在q的位置的情況下 能夠得到L(j)的計測數(shù)據(jù)的概率值，將Pr(qlm(i), 2^(i))作為在假定 符合平均值m(i)、誤差協(xié)方差矩陣Sm(i)的正態(tài)分布之上表示環(huán)境地圖 的點在q的位置的概率值時，[算式10]成立。
Pr(LC/)|m 》-j"{Pr(LC/')|q)'Pr(q|m(0，i:w(/))Wq
當(dāng)假定Pr (q|m(i)， 2m(i))為正態(tài)分布時，成為下面的[算式ll] [算式11]<formula>formula see original document page 45</formula>
另一方面，以z(j)置換L(j), Pr(L(j)lq)能夠以下面的[算式12]近似。
<formula>formula see original document page 45</formula>
這里，Zk(j)依賴于裝載了距離傳感器的移動體例如移動機器人的位 置tr、姿勢Rr。實際上，由于從三維LRF的傳感器坐標(biāo)系中心看到的
q的朝向和計測點L(j)的朝向與圖20所示的不同，所以誤差協(xié)方差矩 陣Zz(j)也需要配合q的朝向旋轉(zhuǎn)轉(zhuǎn)換，但由于從對應(yīng)的環(huán)境地圖上的 點i遠離的q的存在概率低，所以能夠以充分的精度近似。由此， Pr(L(j)lm(i), Sm(i))能夠以下面的[算式13]表示。<formula>formula see original document page 45</formula>
通過簡單的計算，得到下面的[算式14] [算式14]<formula>formula see original document page 46</formula>
其中，a(j)、卩(j)能夠以下面的[算式15]來表示。 [算式15]
<formula>formula see original document page 46</formula>
因此，表示;^莫型點群的點i和計測點群的點j的對應(yīng)的一致度的評 價函數(shù)EM(i, j)，在平均值m(i)、誤差協(xié)方差矩陣Sm(i)+ Sz(j)的正態(tài) 分布中，能夠近似為得到z(j)的概率值。通過使用該評價函數(shù)，能夠進 行考慮了環(huán)境地圖、計測數(shù)據(jù)雙方的對應(yīng)。
關(guān)于計測點和環(huán)境地圖(即，環(huán)境模型)的對應(yīng)進行補充說明。 在上述實施例，由于使用考慮了誤差分布的統(tǒng)計上的評價函數(shù)，所以 不求出評價函數(shù)的值就不能夠確定對應(yīng)點。因此，在環(huán)境地圖上的模 型點群中預(yù)先縮小對應(yīng)的候補，從該候補中基于評價函數(shù)的值求出對 應(yīng)點。具體地，能夠如以下方式確定。 (1)求出與作為對象的計測點j的誤差協(xié)方差矩陣SiXJ)的范圍(例如 標(biāo)準(zhǔn)偏差的三倍的范圍)相交的最上位的體素、和鄰接于該體素的最 上位的體素，將還包含下層的體素的這些體素內(nèi)存在的代表點作為對應(yīng)點的候補。由于體素成為分級結(jié)構(gòu)，所以該候補點的探測幾乎不花 費計算成本。這時，在沒有成為候補的代表點的情況下，視為沒有對 應(yīng)點。將鄰接的體素也加入候補的理由是因為根據(jù)體素內(nèi)的代表點的 位置，誤差協(xié)方差矩陣的范圍有時會溢出至鄰接的體素。
(2) 使用成為候補的體素的代表點i和誤差協(xié)方差矩陣，求出評價函 數(shù)EM(i, j)的值。
(3) 將評價函數(shù)EM(i, j)的值是最大的代表點i作為對應(yīng)點。其中， 在評價函數(shù)的值不足某個閾值的情況下，視為沒有對應(yīng)點。
在本實施例，作為對應(yīng)的評價函數(shù)EM(i, j),采用基于似然的算 式，由于關(guān)于對應(yīng)點的有無，有統(tǒng)計上的明確的判斷尺度，所以不會 有即使在認為對應(yīng)點不存在的情況下還勉強地進行對應(yīng)的情況。再有， 在沒有對應(yīng)點的情況下，解釋為成為對象的計測點是相當(dāng)于至此未計 測的部分的點，追加到環(huán)境地圖。
作為實施方式，對引入外界的三維信息來辨認自身三維位置的自 身位置辨認方法和裝置以及三維形狀的計測方法和裝置進行了說明， 但通過將二維形狀視為三維形狀的特殊情況，也能夠作為引入外界的 二維信息，辨認自身二維位置的自身位置辨認方法和裝置以及二維形 狀的計測方法和裝置的方式來實施。
此外，在所述輸出步驟中，不全部輸出所述體素位置、代表點以 及誤差分布也可，例如在全部沒有它們也能把握三維形狀的情況、或 需要它們中的一個或兩個的情況等情況下，可將所述體素位置、代表 點以及誤差分布的至少任一個輸出到輸出裝置中。
再有，本發(fā)明并不局限于上述實施方式，當(dāng)然能夠在不脫離本發(fā) 明的主旨的范圍內(nèi)進行各種變更。
權(quán)利要求
1.一種自身位置辨認方法，引入外界的三維形狀來辨認自身位置，其特征在于，實施下述步驟數(shù)據(jù)輸入步驟，在新的計測位置處將三維形狀上的坐標(biāo)值輸入到計算機中；模型構(gòu)筑步驟，構(gòu)筑環(huán)境模型，該環(huán)境模型將所述三維形狀所存在的空間區(qū)域分割為由邊界表面相互正交的長方體構(gòu)成的多個體素，并存儲各體素位置；以及匹配步驟，在對應(yīng)于所述坐標(biāo)值的體素的內(nèi)部設(shè)定代表點及其誤差分布并存儲起來，在先前的計測位置的所述三維形狀數(shù)據(jù)不存在的情況下，將新的計測位置辨認為自身位置，在先前的計測位置的所述三維形狀數(shù)據(jù)存在的情況下，實施下述步驟精密對準(zhǔn)步驟，對于針對先前的計測位置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，或者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以鄰近的誤差分布間距離的總和成為最小的方式，進行位置對準(zhǔn)；以及自身位置辨認步驟，根據(jù)所述精密對準(zhǔn)步驟中的旋轉(zhuǎn)量以及平移量來辨認自身位置，進而實施輸出步驟，將所述自身位置輸出到輸出裝置中。
2. 根據(jù)權(quán)利要求1所述的自身位置辨認方法，其特征在于，在所 述匹配步驟中，體素內(nèi)部除了代表點及其誤差分布之外，還設(shè)定并存 儲表示在體素內(nèi)物體存在概率的概率值。
3. 根據(jù)權(quán)利要求1所述的自身位置辨認方法，其特征在于，在所 述精密對準(zhǔn)步驟之前具有粗略對準(zhǔn)步驟，對于針對先前的計測位置 的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，并以具有代 表點的體素間距離的總和成為最小的方式，進行位置對準(zhǔn)，其中該代 表點是與計測數(shù)據(jù)以及誤差分布鄰近的代表點，或者使針對新的計測 位置的環(huán)境模型旋轉(zhuǎn)及平移，并以具有代表點的體素間距離的總和成 為最小的方式，進行位置對準(zhǔn)。
4. 根據(jù)權(quán)利要求1所述的自身位置辨認方法，其特征在于，在所 述精密對準(zhǔn)步驟之前具有粗略對準(zhǔn)步驟，對于針對先前的計測位置 的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，并以具有代表點的體素的概率值的總和成為最大的方式，進行位置對準(zhǔn)，其中該 代表點是與計測數(shù)據(jù)以及誤差分布鄰近的代表點，或者使針對新的計 測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以鄰近的體素所具有的概率值的差 的總和成為最小的方式，進行位置對準(zhǔn)。
5. 根據(jù)權(quán)利要求1所述的自身位置辨認方法，其特征在于，在所 述數(shù)據(jù)輸入步驟之后具有探測范圍限定步驟，根據(jù)過去的計測位置 的變化推定當(dāng)前的計測位置，或通過能夠取得當(dāng)前的計測位置的傳感 器來取得，或不僅利用計測數(shù)據(jù)的距離值還利用反射強度值，限定核 對的范圍。
6. 根據(jù)權(quán)利要求1所述的自身位置辨認方法，其特征在于，在所 述自身位置辨認步驟中，根據(jù)先前的計測位置的位置以及姿勢來辨認 新的計測位置的六自由度位置。
7. 根據(jù)權(quán)利要求1所述的自身位置辨認方法，其特征在于，在所 述精密對準(zhǔn)步驟中，將誤差分布交叉的情況作為同一計測點，將根據(jù) 分布的 一 致度求得的加權(quán)乘以該情況的距離值，計算誤差分布間的距 離。
8. 根據(jù)權(quán)利要求1所述的自身位置辨認方法，其特征在于，在所 述模型構(gòu)筑步驟中，在將最大的體素設(shè)定為相當(dāng)于必要最小限度的分 辨能力的大小，而且單一的體素內(nèi)存在多個被計測點的情況下，以在 單一的體素內(nèi)僅存在單一的被計測點的方式，將該體素進一步分割并分級地分割成多個體素。
9. 根據(jù)權(quán)利要求1所述的自身位置辨認方法，其特征在于，在所 述自身位置辨認步驟之后具有更新所述環(huán)境模型的模型更新步驟，在 該;漢型更新步驟中，探測對應(yīng)于新輸入的被計測點的坐標(biāo)值的體素，作為在原點和被計測點之間不存在物體的，再設(shè)定或擦除位于其 間的體素內(nèi)的代表點和誤差分布。
10. 根據(jù)權(quán)利要求1所述的自身位置辨認方法，其特征在于，在所 述自身位置辨認步驟之后具有更新所述環(huán)境模型的模型更新步驟，在 該模型更新步驟中，探測對應(yīng)于新輸入的被計測點的坐標(biāo)值的體素，在該體素內(nèi)沒有代表點的情況下，將所述坐標(biāo)值和誤差分布設(shè)定 為代表點的坐標(biāo)值和誤差分布。
11. 根據(jù)權(quán)利要求1所述的自身位置辨認方法，其特征在于，在所述自身位置辨認步驟之后具有更新所述環(huán)境模型的模型更新步驟，在 該模型更新步驟中，探測對應(yīng)于新輸入的被計測點的坐標(biāo)值的體素，在所述體素內(nèi)有已經(jīng)設(shè)定了的代表點的情況下，比較新取得的誤 差分布和已經(jīng)設(shè)定了的體素內(nèi)的誤差分布，在誤差分布相互重疊的情況下，根據(jù)兩誤差分布再設(shè)定新的誤差 分布和新的代表點，在誤差分布不相互重疊的情況下，以在單一的體素內(nèi)僅存在單一 的代表點的方式，將該體素進一 步分割并分級地分割為多個體素。
12. 根據(jù)權(quán)利要求1所述的自身位置辨認方法，其特征在于，在所 述自身位置辨認步驟中，將自身位置的誤差分布與自身位置的辨認一 起進行辨認，在所述輸出步驟之前，根據(jù)當(dāng)前的自身位置和誤差分布 以及辨認了的自身位置和誤差分布，通過卡爾曼濾波器，來校正自身 位置和誤差分布，在輸出步驟中，輸出自身位置和誤差分布。
13. 根據(jù)權(quán)利要求1所述的自身位置辨認方法，其特征在于，在所 述模型更新步驟中，比較所述新取得的誤差分布和所述已經(jīng)設(shè)定了的 體素內(nèi)的誤差分布，在誤差分布相互重疊的情況下，根據(jù)兩誤差分布 再設(shè)定新的誤差分布和新的代表點，其結(jié)果是新的代表點移動到其他 體素內(nèi)，此時，在該其他體素內(nèi)沒有代表點的情況下，在該其他體素的內(nèi)部設(shè)定 該新的誤差分布和該新的代表點，在該其他體素內(nèi)有已經(jīng)設(shè)定了的代表點的情況下，比較該新的誤差分布和已經(jīng)設(shè)定了的該其他體素內(nèi)的誤差分布，(A)在誤差分布相互重疊的情況下，根據(jù)兩誤差分布、或根據(jù)兩誤差分布和體素內(nèi)已經(jīng) 設(shè)定了的代表點和新輸入的被計測點的坐標(biāo)值，再設(shè)定新的誤差分布和新的代表點，(B)在誤差分布不相互重疊的情況下，以在單一的體素內(nèi)僅存在單一的代表點的方式，將該體素進一步分割并分級地分割 為多個體素。
14. 根據(jù)權(quán)利要求1所述的自身位置辨認方法，其特征在于，在所 述自身位置辨認步驟之后具有更新所述環(huán)境模型的模型更新步驟，在該模型更新步驟中，根據(jù)新輸入的被計測點的坐標(biāo)值及其誤差 分布、和已經(jīng)設(shè)定了的體素內(nèi)的代表點及其誤差分布，通過卡爾曼濾 波器取得新的代表點和誤差分布并進行再設(shè)定。
15. 根據(jù)權(quán)利要求1所述的自身位置辨認方法，其特征在于，在所 述精密對準(zhǔn)步驟中，代替以與所述鄰近的誤差分布間距離有關(guān)的評價 值成為最小的方式進行位置對準(zhǔn)，而是以與 一致度有關(guān)的評價值成為 最大的方式，對于針對先前的計測位置的環(huán)境模型，使新的計測數(shù)據(jù) 以及誤差分布旋轉(zhuǎn)及平移，或者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn) 及平移，從而進行位置對準(zhǔn)，其中該一致度是通過基于所述鄰近的誤 差分布間的極大似然推定值而確定的。
16. 根據(jù)權(quán)利要求15所述的自身位置辨認方法，其特征在于，所 述與 一 致度有關(guān)的評價值的計算式以下面的[算式16 ]表示，在該式中，計測點j和環(huán)境模型上的代表點i對應(yīng)起來，能得到該 計測點j的計測數(shù)據(jù)的概率為EM (i, j)， co(j)在環(huán)境;f莫型中存在與計 測點j對應(yīng)起來的代表點的情況下是1,在除此之外的情況下為0。
17. —種自身位置辨認方法，引入外界的三維形狀來辨認自身位 置，其特征在于，實施下述步驟數(shù)據(jù)輸入步驟，在新的計測位置處將三維形狀上的坐標(biāo)值輸入到 計算機中；模型構(gòu)筑步驟，構(gòu)筑環(huán)境模型，該環(huán)境模型將所述三維形狀所存 在的空間區(qū)域分割為由邊界表面相互正交的長方體構(gòu)成的多個體素， 并存儲各體素位置；以及匹配步驟，在對應(yīng)于所述坐標(biāo)值的體素的內(nèi)部設(shè)定代表點及其誤 差分布并存儲起來，在先前的計測位置的所述三維形狀數(shù)據(jù)不存在的情況下，將新的 計測位置辨認為自身位置，在先前的計測位置的所述三維形狀數(shù)據(jù)存在的情況下，實施下述 步驟精密對準(zhǔn)步驟，對于針對先前的計測位置的環(huán)境模型，使新的 計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，或者使針對新的計測位置的環(huán)境[算式16]<formula>formula see original document page 5</formula>模型旋轉(zhuǎn)及平移，并以與鄰近的誤差分布間距離有關(guān)的評價值成為最小的方式，進行位置對準(zhǔn)；以及自身位置辨認步驟，根據(jù)所述精密對 準(zhǔn)步驟中的旋轉(zhuǎn)量以及平移量來辨認自身位置。
18. 根據(jù)權(quán)利要求17所述的自身位置辨認方法，其特征在于，實 施輸出步驟，將所述自身位置輸出到輸出裝置中。
19. 根據(jù)權(quán)利要求17所述的自身位置辨認方法，其特征在于，在 所述匹配步驟中，體素內(nèi)部除了代表點及其誤差分布之外，還設(shè)定并 存儲表示在體素內(nèi)物體存在概率的概率值。
20. 根據(jù)權(quán)利要求17所述的自身位置辨認方法，其特征在于，在 所述精密對準(zhǔn)步驟之前具有粗略對準(zhǔn)步驟，對于針對先前的計測位 置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，并以與具 有代表點的體素間距離有關(guān)的評價值成為最小的方式，進行位置對準(zhǔn)， 其中該代表點是與計測數(shù)據(jù)以及誤差分布鄰近的代表點，或者使針對 新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與具有代表點的體素間距 離有關(guān)的評價值成為最小的方式，進行位置對準(zhǔn)。
21. 根據(jù)權(quán)利要求17所述的自身位置辨認方法，其特征在于，在 所述精密對準(zhǔn)步驟之前具有粗略對準(zhǔn)步驟，對于針對先前的計測位 置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，并以與具 有代表點的體素的概率值有關(guān)的評價值成為最大的方式，進行位置對 準(zhǔn)，其中該代表點是與計測數(shù)據(jù)以及誤差分布鄰近的代表點，或者使 針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與鄰近的體素所具有 的概率值的差有關(guān)的評價值成為最小的方式，進行位置對準(zhǔn)。
22. —種三維形狀的計測方法，根據(jù)外界的三維形狀的被計測點的 坐標(biāo)值來復(fù)原三維形狀，并輸出三維形狀數(shù)據(jù)，其特征在于，實施下 述步驟數(shù)據(jù)輸入步驟，在新的計測位置處將三維形狀上的坐標(biāo)值輸入到 計算機中；模型構(gòu)筑步驟，構(gòu)筑環(huán)境模型，該環(huán)境模型將所述三維形狀所存 在的空間區(qū)域分割為由邊界表面相互正交的長方體構(gòu)成的多個體素， 并存儲各體素位置；以及匹配步驟，在對應(yīng)于所述坐標(biāo)值的體素的內(nèi)部設(shè)定代表點及其誤 差分布并存儲起來，在先前的計測位置的所述三維形狀數(shù)據(jù)不存在的情況下，將新的 計測位置辨認為自身位置，在先前的計測位置的所述三維形狀數(shù)據(jù)存在的情況下，實施下述步驟精密對準(zhǔn)步驟，對于針對先前的計測位置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，或者使針對新的計測位置的環(huán)境 模型旋轉(zhuǎn)及平移，并以與鄰近的誤差分布間距離有關(guān)的評價值成為最小的方式，進行位置對準(zhǔn)；以及自身位置辨認步驟，根據(jù)所述精密對 準(zhǔn)步驟中的旋轉(zhuǎn)量以及平移量來辨認自身位置，所述方法還具有輸出步驟，將所述自身位置、基于所述自身位 置的所述體素位置、代表點以及誤差分布的至少任一個輸出到輸出裝 置中。
23. 根據(jù)權(quán)利要求22所述的三維形狀的計測方法，其特征在于， 在所述輸出步驟中，將所述體素位置、代表點位置以及誤差分布位置 的至少任一個作為三維形狀的計測值輸出到輸出裝置中，并且將表示 該計測值的可靠性或精度的指標(biāo)基于該體素內(nèi)部的誤差分布的大小， 輸出到輸出裝置中。
24. 根據(jù)權(quán)利要求22所述的三維形狀的計測方法，其特征在于， 在所述輸出步驟中，將所述體素位置、代表點位置以及誤差分布位置 的至少任 一 個作為三維形狀的計測值輸出到輸出裝置中時，在該體素 內(nèi)部的誤差分布的大小比規(guī)定的基準(zhǔn)值大的情況下，該計測值的可靠 性或精度定為比規(guī)定的基準(zhǔn)低，不將該體素的所述計測值輸出到輸出 裝置中。
25. 根據(jù)權(quán)利要求22所述的三維形狀的計測方法，其特征在于， 在所述匹配步驟之后具有更新所述環(huán)境模型的模型更新步驟，在該模 型更新步驟中，探測對應(yīng)于新輸入的^皮計測點的坐標(biāo)值的體素，在該體素內(nèi)沒有代表點的情況下，將所述坐標(biāo)值和誤差分布設(shè)定 為代表點的坐標(biāo)值和誤差分布。
26. 根據(jù)權(quán)利要求22所述的三維形狀的計測方法，其特征在于， 在所述匹配步驟之后具有更新所述環(huán)境模型的模型更新步驟，在該模 型更新步驟中，探測對應(yīng)于新輸入的#皮計測點的坐標(biāo)值的體素，在所述體素內(nèi)有已經(jīng)設(shè)定了的代表點的情況下，比較新取得的誤 差分布和已經(jīng)設(shè)定了的體素內(nèi)的誤差分布，在誤差分布相互重疊的情況下，根據(jù)兩誤差分布、或根據(jù)兩誤差 分布和體素內(nèi)已經(jīng)設(shè)定了的代表點和新輸入的被計測點的坐標(biāo)值，再設(shè)定新的誤差分布和新的代表點，在誤差分布不相互重疊的情況下，以在單一的體素內(nèi)僅存在單一 的代表點的方式，將該體素進一步分割并分級地分割為多個體素。
27. 根據(jù)權(quán)利要求22所述的三維形狀的計測方法，其特征在于， 在所述匹配步驟之后具有更新所述環(huán)境模型的模型更新步驟，在該模 型更新步驟中，探測對應(yīng)于新輸入的被計測點的坐標(biāo)值的體素，在所 述體素內(nèi)的代表點以及誤差分布的至少任一個被新設(shè)定、或被再設(shè)定、 或?qū)⒃擉w素進一步分割并分級地分割為多個體素的情況下，在所述輸 出步驟中，將該體素的代表點位置作為三維形狀的計測值輸出到輸出 裝置中。
28. 根據(jù)權(quán)利要求22所述的三維形狀的計測方法，其特征在于， 在所述輸出步驟中，將從距離傳感器的位置出發(fā)距離傳感器能進行位 置計效'J的范圍的環(huán)境模型內(nèi)的體素的代表點位置，作為三維形狀的計 測值輸出到輸出裝置中。
29. —種自身位置辨認裝置，引入外界的三維形狀來辨認自身位 置，其特征在于，具備數(shù)據(jù)輸入裝置，將三維形狀上的坐標(biāo)值輸入到計算機中； 模型構(gòu)筑裝置，構(gòu)筑環(huán)境模型，該環(huán)境模型將所述三維形狀所存在的空間區(qū)域分割為由邊界表面相互正交的長方體構(gòu)成的多個體素，并存儲各體素位置；匹配裝置，在對應(yīng)于所述坐標(biāo)值的體素的內(nèi)部設(shè)定代表點及其誤 差分布并存儲起來；以及輸出裝置，將自身位置輸出到輸出裝置中，在新的計測位置處，在先前的計測位置的所述三維形狀的數(shù)據(jù)不 存在的情況下，將新的計測位置辨認為自身位置，在新的計測位置處，在先前的計測位置的所述三維形狀數(shù)據(jù)存在 的情況下，對于針對先前的計測位置的環(huán)境模型，使針對新的計測位置的環(huán) 境模型旋轉(zhuǎn)及平移，并以鄰近的誤差分布間距離的總和成為最小的方 式，進行位置對準(zhǔn)，根據(jù)所述位置對準(zhǔn)中的旋轉(zhuǎn)量以及平移量來辨認自身位置。
30. —種自身位置辨認裝置，引入外界的三維形狀來辨認自身位置，其特征在于，具備數(shù)據(jù)輸入裝置，將三維形狀上的坐標(biāo)值輸入到計算機中；模型構(gòu)筑裝置，構(gòu)筑環(huán)境模型，該環(huán)境模型將所述三維形狀所存 在的空間區(qū)域分割為由邊界表面相互正交的長方體構(gòu)成的多個體素， 并存儲各體素位置；匹配裝置，在對應(yīng)于所述坐標(biāo)值的體素的內(nèi)部設(shè)定代表點及其誤 差分布并存儲起來；以及輸出裝置，將自身位置輸出到輸出裝置中，在新的計測位置處，在先前的計測位置的所述三維形狀數(shù)據(jù)不存 在的情況下，將新的計測位置辨認為自身位置，在新的計測位置處，在先前的計測位置的所述三維形狀數(shù)據(jù)存在 的情況下，對于針對先前的計測位置的環(huán)境模型，使針對新的計測位置的環(huán) 境模型旋轉(zhuǎn)及平移，并以與鄰近的誤差分布間距離有關(guān)的評價值成為 最小的方式，進行位置對準(zhǔn)，根據(jù)所述位置對準(zhǔn)中的旋轉(zhuǎn)量以及平移量來辨認自身位置。
31. —種三維形狀的計測裝置，根據(jù)三維形狀上的被計測點的坐標(biāo) 值來復(fù)原三維形狀，并輸出三維形狀數(shù)據(jù)，其特征在于，具備數(shù)據(jù)輸入裝置，將三維形狀上的坐標(biāo)值輸入到計算機中； 模型構(gòu)筑裝置，構(gòu)筑環(huán)境模型，該環(huán)境模型將所述三維形狀所存在的空間區(qū)域分割為由邊界表面相互正交的長方體構(gòu)成的多個體素，并存儲各體素位置；匹配裝置，在對應(yīng)于所述坐標(biāo)值的體素的內(nèi)部設(shè)定代表點及其誤 差分布并存儲起來；以及 數(shù)據(jù)傳輸裝置， 其中，在該數(shù)據(jù)傳輸裝置中，在新的計測位置處，在先前的計測位置的所述三維形狀數(shù)據(jù)不存 在的情況下，將新的計測位置辨認為自身位置，在新的計測位置處，在先前的計測位置的所述三維形狀數(shù)據(jù)存在 的情況下，對于針對先前的計測位置的環(huán)境模型，使針對新的計測位置的環(huán) 境模型旋轉(zhuǎn)及平移，并以與鄰近的誤差分布間距離有關(guān)的評價值成為 最小的方式，進行位置對準(zhǔn)，根據(jù)所述位置對準(zhǔn)中的旋轉(zhuǎn)量以及平移量來辨認自身位置， 進而將所述自身位置、基于所述自身位置的所述體素位置、代表 點以及誤差分布的至少任 一 個輸出到輸出裝置中。
全文摘要
本發(fā)明實施下述步驟步驟(S1)，在新的計測位置處將三維形狀上的坐標(biāo)值輸入到計算機中；步驟(S4)，構(gòu)筑環(huán)境模型，該環(huán)境模型將三維形狀所存在的空間區(qū)域分割為由立方體構(gòu)成的多個體素，并存儲各位置；以及步驟(S5)，在對應(yīng)于坐標(biāo)值的體素的內(nèi)部設(shè)定代表點及其誤差分布并存儲起來。接著，在先前的計測位置的數(shù)據(jù)存在的情況下，實施精密對準(zhǔn)步驟(S7)，對于針對先前的計測位置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，或者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與鄰近的誤差分布間距離有關(guān)的評價值成為最小的方式，進行位置對準(zhǔn)。根據(jù)精密對準(zhǔn)步驟中的旋轉(zhuǎn)量以及平移量來辨認自身位置。
文檔編號G06T17/10GK101331379SQ20068004746
公開日2008年12月24日 申請日期2006年12月15日 優(yōu)先權(quán)日2005年12月16日
發(fā)明者寺田英雄, 林俊寬, 河野幸弘 申請人:株式會社Ihi