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