專利名稱：三維形狀數(shù)據(jù)的位置對準(zhǔn)方法和裝置的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種用于從多個計測位置來計測靜止的三維形狀并綜合其距離數(shù)據(jù)來進(jìn)行位置對準(zhǔn)的三維形狀數(shù)據(jù)的位置對準(zhǔn)(position matching; aligning)方法和裝置。
背景技術(shù)：
在本發(fā)明中將對從多視點計測靜態(tài)三維形狀的多個距離圖像的幾 何學(xué)位置關(guān)系進(jìn)行推定的過程稱為"位置對準(zhǔn)"。三維形狀的計測技術(shù)已普及，三維形狀數(shù)據(jù)用于各種各樣的應(yīng)用 領(lǐng)域，例如機械零件/工件等的形狀計測、移動機器人中的自身位置的 辨認(rèn)、地形/結(jié)構(gòu)物的計測等。在進(jìn)行某個物體的三維計測來復(fù)原三維形狀的情況下，從一個視 點(計測位置)的計測能夠獲得面對該視點的表面信息，但不能計測 物體的背面。因此，為了復(fù)原三維形狀，需要將來自多個視點的計測 及其計測結(jié)果精度良好地進(jìn)行位置對準(zhǔn)。為了對從多個視點取得的數(shù)據(jù)進(jìn)行位置對準(zhǔn)，視點和計測對象的 位置關(guān)系必須是明顯的。作為取得該視,存、和計測對象的位置關(guān)系的方 法，存在將物體配置在臺子上并使其旋轉(zhuǎn)的方法，或者使具有連桿機 構(gòu)(link mechanism)的臂狀物等帶有視點的方法，但均對位置關(guān)系的 精度有極限。此外，假設(shè)即使能夠得到精度，也會發(fā)生對計測物體的 大小的限制。因此作為不受位置關(guān)系的精度的影響，并且對計測物體的大小也 沒有限制的辦法，有多種多樣的一邊比較先得到的計測數(shù)據(jù)和之后得 到的計測數(shù)據(jù)一邊進(jìn)行位置對準(zhǔn)的辦法被提出。其中,在對應(yīng)點是已知的情況下，作為通過旋轉(zhuǎn)和平移(translate) 的平方誤差的最小化問題，能夠容易地求得。但是， 一般對應(yīng)點并不 是已知的。作為對應(yīng)點不是已知的情況下的位置對準(zhǔn)辦法，根據(jù)不變特征量 的匹配(matching )、直接法以及ICP算法已經(jīng)一皮提出(例如，非專利文獻(xiàn)1、 2)。"根據(jù)不變特征量的匹配"是對形狀利用不變的局部特征點(微分特 征等)來進(jìn)行兩個數(shù)據(jù)的匹配。"直接法"是關(guān)于三維圖像求出二維圖像的光流(optical flow)。 "ICP (Iterative Closest Points:迭代最近點)算法"是求出相對于先 前的計測數(shù)據(jù)的其后的計測數(shù)據(jù)的最鄰近的點， 一 邊進(jìn)行旋轉(zhuǎn)/平移一 邊以其距離的總和為最小的狀態(tài)變成一致狀態(tài)的方式來求解。 此外，作為與本發(fā)明有關(guān)的技術(shù)，公開有非專利文獻(xiàn)3。 非專利文獻(xiàn)l:增田健等、"來自多個距離圖像的形狀模型生成技術(shù)" 非專利文獻(xiàn)2: Paul J. Besl, "A Method for Registration of 3-D Shapes", IEEE Transactions of Pattern Analysis and Mechanical Intelligence, Vol.l4， No.2，F(xiàn)ebruary 1992非專利文獻(xiàn)3:關(guān)本清英等、"三維激光雷達(dá)的開發(fā)"，石川島播磨技報 Vol.43 No.4 (2003-7)發(fā)明內(nèi)容發(fā)明要解決的問題在使用三維激光雷達(dá)那樣的距離傳感器的情況下，被計測的三維 形狀上的被計測點成為在橫向以及垂直方向上離散的點群。該點群的 間隔，在距計測點的距離例如是50m的情況下，被計測點的間隔例如 達(dá)到在橫向是大約315mm，在垂直方向是大約525mm。此外，在從多個計測位置來計測靜止的三維形狀的情況下，在三 維激光雷達(dá)那樣的距離傳感器，每個計測位置，通常被計測點的位置 不同。進(jìn)而，這樣的距離傳感器一般在測定距離上具有大約20cm前后的 誤差。因此，在使用三維激光雷達(dá)那樣的距離傳感器的情況下，有以下 的限制條件A~C。條件A:計測數(shù)據(jù)中點數(shù)少(例如，l幀-166x50點)條件B:計測數(shù)據(jù)中包含誤差(例如，在測定距離中是大約20cm前后)條件C:計測數(shù)據(jù)未必測量相同的計測點。即，得到的距離數(shù)據(jù)是在橫向以及垂直方向上離散的點群，由于每一計測的位置不同，所以沒有對應(yīng)點，在測定距離中包含比較大的 誤差。在以上述的"根據(jù)不變特征量的匹配，，或"直接法"來處理這樣的距 離數(shù)據(jù)的情況下，由于是離散的點群，計測點少，沒有對應(yīng)點，計測 誤差大，所以正確的位置對準(zhǔn)幾乎不可能。此外，在使用ICP算法的情況下，點群的位置對準(zhǔn)在原理上是可 能的，但有以下問題。 (1 )誤差的累積IC P算法是兩個距離數(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é)果。 發(fā)明概要本發(fā)明是為了解決上述問題而做出的。即，本發(fā)明的目的是提供一 種三維形狀數(shù)據(jù)的位置對準(zhǔn)方法和裝置，其中，即使在從多個計測位 置來計測靜止的三維形狀的距離數(shù)據(jù)是空間上離散的點群，每一計測 的位置不同且沒有對應(yīng)點，包含比較大的誤差的情況下，也沒有誤差 的累積，并以較少的計算量就能夠進(jìn)行正確的位置對準(zhǔn)。根據(jù)本發(fā)明，提供一種三維形狀數(shù)據(jù)的位置對準(zhǔn)方法，用于從多個 計測位置來計測靜止的三維形狀,綜合其距離數(shù)據(jù)以進(jìn)行位置對準(zhǔn)， 其特征在于，實施下述步驟數(shù)據(jù)輸入步驟，在新的計測位置處將三 維形狀上的坐標(biāo)值輸入到計算機中；模型構(gòu)筑步驟，構(gòu)筑環(huán)境模型， 該環(huán)境模型將所述三維形狀所存在的空間區(qū)域分割為邊界表面相互正 交的長方體構(gòu)成的多個體素(voxel),存儲各體素位置；以及匹配步驟， 在對應(yīng)于所述坐標(biāo)值的體素的內(nèi)部設(shè)定代表點及其誤差分布并存儲起來，還具有下述步驟精密對準(zhǔn)步驟，對于針對先前的計測位置的環(huán) 境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，或者使針對新的 計效'J位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與鄰近的誤差分布間距離有關(guān) 的評價值(例如，該距離的總和)成為最小的方式，進(jìn)行位置對準(zhǔn)； 以及輸出步驟，將所述體素位置、代表點以及誤差分布輸出到輸出裝 置中。再有，上述與距離有關(guān)的評價值，代替該距離的總和，是該距離 的平均值、該距離的二次方的總和、或該距離的最大值也可，是其他 的適合的評價值也可。根據(jù)本發(fā)明的優(yōu)選實施方式，在所述匹配步驟中，在體素內(nèi)部除 了代表點及其誤差分布之外，還設(shè)定并存儲表示在體素內(nèi)物體存在概 率的概率值。此外，在所述精密對準(zhǔn)步驟之前具有粗略對準(zhǔn)步驟，對于針對 先前的計測位置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平 移，并以與具有代表點的體素間距離有關(guān)的評價值(例如，該距離的 總和)成為最小的方式，進(jìn)行位置對準(zhǔn)，其中該代表點是與計測數(shù)據(jù) 以及誤差分布鄰近的代表點，或者使針對新的計測位置的環(huán)境模型旋 轉(zhuǎn)及平移，并以與具有代表點的體素間距離有關(guān)的評價值(例如，該 距離的總和)成為最小的方式，進(jìn)行位置對準(zhǔn)。再有，與具有和計測數(shù)據(jù)以及誤差分布鄰近的代表點的體素間距離有關(guān)的上述評價值，代替該距離的總和，是該距離的平均值、該距 離的二次方的總和、或該距離的最大值也可，是其他的適合的評價值 也可。此外，與具有代表點的體素間距離有關(guān)的上述評價值，代替該距 離的總和，是該距離的平均值、該距離的二次方的總和、或該距離的 最大值也可，是其他的適合的評價值也可?；蛘?，在所述精密對準(zhǔn)步驟之前具有粗略對準(zhǔn)步驟，對于針對 先前的計測位置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平 移，并以與具有代表點的體素的概率值有關(guān)的評價值(例如，該概率 值的總和)成為最大的方式，進(jìn)行位置對準(zhǔn)，其中該代表點是與計測 數(shù)據(jù)以及誤差分布鄰近的代表點，或者使針對新的計測位置的環(huán)境模 型旋轉(zhuǎn)及平移，并以與鄰近的體素所具有的概率值的差有關(guān)的評價值(例如，該概率值的差的總和)成為最小的方式，進(jìn)行位置對準(zhǔn)。再有，與具有和計測數(shù)據(jù)以及誤差分布鄰近的代表點的體素的概率值有關(guān)的上述評價值，代替該概率值的總和，是該概率值的平均值、 該概率值的二次方的總和、或該概率值的最小值也可，是其他的適合 的評-階值也可。此外，與鄰近的體素所具有的概率值的差有關(guān)的上述評價值，代 替該概率值的差的總和，是該概率值的差的平均值、該概率值的差的 二次方的總和、或該概率值的差的最大值也可，是其他的適合的評價 值也可。此外，在所述數(shù)據(jù)輸入步驟之后具有探測范圍限定步驟，根據(jù) 過去的計測位置的變化推定當(dāng)前的計測位置，或通過能夠取得當(dāng)前的 計測位置的傳感器來取得，或不僅利用計測數(shù)據(jù)的距離值還利用反射 強度值，限定核對的范圍。此外，在所述模型構(gòu)筑步驟中，在將最大的體素設(shè)定為相當(dāng)于必 要最小限度的分辨能力的大小，而且單 一的體素內(nèi)存在多個被計測點 的情況下，以在單一的體素內(nèi)僅存在單一的被計測點的方式，將該體 素進(jìn)一步分割并分級地分割成多個體素。此外，在所述精密對準(zhǔn)步驟之后具有更新所述環(huán)境模型的模型更 新步驟，在該模型更新步驟中，探測對應(yīng)于新輸入的被計測點的坐標(biāo) 值的體素，作為在原點和被計測點之間不存在物體的，再設(shè)定或擦除 位于其間的體素內(nèi)的代表點和誤差分布。此外，在所述精密對準(zhǔn)步驟之后具有更新所述環(huán)境模型的模型更 新步驟，在該模型更新步驟中，探測對應(yīng)于新輸入的被計測點的坐標(biāo) 值的體素，在該體素內(nèi)沒有代表點的情況下，將所述坐標(biāo)值和誤差分 布設(shè)定為代表點的坐標(biāo)值和誤差分布。此外，在所述精密對準(zhǔn)步驟之后具有更新所述環(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ìn)一 步分割并分級地分割為多個體素。體素的分割例如使用八叉樹或K-D樹。在所述模型更新步驟中，比較所述新取得的誤差分布和所述已經(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)僅存在單一的代 表點的方式，將該體素進(jìn)一步分割并分級地分割為多個體素。根據(jù)本發(fā)明的另外的實施方式，在所述精密對準(zhǔn)步驟之后具有更 新所述環(huán)境模型的模型更新步驟，在該模型更新步驟中，根據(jù)新輸入 的被計測點的坐標(biāo)值及其誤差分布、和已經(jīng)設(shè)定了的體素內(nèi)的代表點 及其誤差分布，通過卡爾曼濾波器取得新的代表點和誤差分布并進(jìn)行 再設(shè)定。在所述精密對準(zhǔn)步驟中，代替以與所述鄰近的誤差分布間距離有 關(guān)的評價值成為最小的方式進(jìn)行位置對準(zhǔn)，而是以與 一致度有關(guān)的評 價值(例如，該一致度的總乘積)成為最大的方式，對于針對先前的 計測位置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，或 者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，從而進(jìn)行位置對準(zhǔn)， 其中該一致度是通過基于所述鄰近的誤差分布間的極大似然推定值而 確定的。所述與 一致度有關(guān)的評價值的計算式以下面的[算式1 ]表示， [算式1]境模型上的代表點i對應(yīng)起來，能得到該 計測點j的計測數(shù)據(jù)的概率為EM (i, j), co(j)在環(huán)境模型中與計測點 j對應(yīng)起來的代表點存在的情況下是1,在除此之外的情況下為0。此外，根據(jù)本發(fā)明，提供一種三維形狀數(shù)據(jù)的位置對準(zhǔn)裝置，用 于從多個計測位置來計測靜止的三維形狀，綜合其距離數(shù)據(jù)以進(jìn)行位 置對準(zhǔn)，其特征在于，具備數(shù)據(jù)輸入裝置，將三維形狀上的坐標(biāo)值 輸入到計算機中；模型構(gòu)筑裝置，構(gòu)筑環(huán)境模型，該環(huán)境模型將所述 三維形狀所存在的空間區(qū)域分割為由邊界表面相互正交的長方體構(gòu)成 的多個體素，并存儲各體素位置；匹配裝置，在對應(yīng)于所述坐標(biāo)值的 體素的內(nèi)部設(shè)定代表點及其誤差分布并存儲起來；以及數(shù)據(jù)傳輸裝置， 將所述體素位置、代表點以及誤差分布輸出到輸出裝置中，對于針對 先前的計測位置的環(huán)境模型，使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及 平移，并以與鄰近的誤差分布間距離有關(guān)的評價值(例如，該距離的 總和)成為最小的方式，進(jìn)行位置對準(zhǔn)。所述匹配裝置在體素的內(nèi)部除了代表點及其誤差分布之外，還設(shè) 定并存儲表示在體素內(nèi)物體存在概率的概率值。所述三維形狀數(shù)據(jù)的位置對準(zhǔn)裝置，在所述位置對準(zhǔn)(所述精密 對準(zhǔn)步驟)之前，進(jìn)行粗略對準(zhǔn)步驟，對于針對先前的計測位置的 環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，以與具有代表 點的體素間距離有關(guān)的評價值(例如，該距離的總和)成為最小的方 式，進(jìn)行位置對準(zhǔn)，其中該代表點是與計測數(shù)據(jù)以及誤差分布鄰近的 代表點，或者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，以與具有 代表點的體素間距離有關(guān)的評價值(例如，該距離的總和)成為最小 的方式，進(jìn)行位置對準(zhǔn)。所述三維形狀數(shù)據(jù)的位置對準(zhǔn)裝置，在所述精密對準(zhǔn)步驟之前， 進(jìn)行粗略對準(zhǔn)步驟，對于針對先前的計測位置的環(huán)境模型，使新的 計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，并以與具有代表點的體素的概率 值有關(guān)的評價值(例如，該概率值的總和)成為最大的方式，進(jìn)行位 置對準(zhǔn)，其中該代表點是與計測數(shù)據(jù)以及誤差分布鄰近的代表點，或 者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與鄰近的體素所 具有的概率值的差有關(guān)的評價值(例如，該概率值的差的總和)成為 最小的方式，進(jìn)行位置對準(zhǔn)。所述三維形狀數(shù)據(jù)的位置對準(zhǔn)裝置，在所述數(shù)據(jù)輸入步驟之后， 進(jìn)行探測范圍限定步驟，根據(jù)過去的計測位置的變化推定當(dāng)前的計 測位置，或通過能夠取得當(dāng)前的計測位置的傳感器來取得，或不僅利 用計測數(shù)據(jù)的距離值還利用反射強度值，限定核對的范圍。所述三維形狀數(shù)據(jù)的位置對準(zhǔn)裝置，在所述精密對準(zhǔn)步驟中，將 誤差分布交叉的情況作為同一計測點，將根據(jù)分布的一致度求得的加權(quán)(weight)乘以該情況的距離值，計算誤差分布間的距離。所述模型構(gòu)筑裝置，在將最大的體素設(shè)定為相當(dāng)于必要最小限度 的分辨能力的大小，而且單一的體素內(nèi)存在多個被計測點的情況下， 以在單一的體素內(nèi)僅存在單一的被計測點的方式，將該體素進(jìn)一步分 割并分級地分割成多個體素。所述三維形狀數(shù)據(jù)的位置對準(zhǔn)裝置，在所述精密對準(zhǔn)步驟之后， 進(jìn)行更新所述環(huán)境模型的模型更新步驟，在該模型更新步驟中，探測 對應(yīng)于新輸入的被計測點的坐標(biāo)值的體素，作為在原點和被計測點之 間不存在物體的，再設(shè)定或擦除位于其間的體素內(nèi)的代表點和誤差分 布。所述三維形狀數(shù)據(jù)的位置對準(zhǔn)裝置，在所述精密對準(zhǔn)步驟之后具 有更新所述環(huán)境模型的模型更新裝置，該模型更新裝置探測對應(yīng)于新 輸入的被計測點的坐標(biāo)值的體素，在該體素內(nèi)沒有代表點的情況下， 將所述坐標(biāo)值和誤差分布設(shè)定為代表點的坐標(biāo)值和誤差分布。所述三維形狀數(shù)據(jù)的位置對準(zhǔn)裝置，在所述精密對準(zhǔn)步驟之后具 有更新所述環(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ìn)一步分割 并分級地分割為多個體素。所述模型更新裝置比較所述新取得的誤差分布和所述已經(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)僅存在單一的代表點 的方式，將該體素進(jìn)一步分割并分級地分割為多個體素。根據(jù)本發(fā)明的另外的實施方式，所述三維形狀數(shù)據(jù)的位置對準(zhǔn)裝 置，在所述精密對準(zhǔn)步驟之后具有更新所述環(huán)境模型的模型更新裝置， 該模型更新裝置根據(jù)新輸入的被計測點的坐標(biāo)值及其誤差分布、和已 經(jīng)設(shè)定了的體素內(nèi)的代表點及其誤差分布，通過卡爾曼濾波器取得新 的代表點和誤差分布并進(jìn)行再設(shè)定。所述三維形狀數(shù)據(jù)的位置對準(zhǔn)裝置，在所述精密對準(zhǔn)步驟中，代 替以與所述鄰近的誤差分布間距離有關(guān)的評價值(例如，該一致度的 總乘積)成為最小的方式進(jìn)行位置對準(zhǔn)，而是以與一致度有關(guān)的評價 值(例如，該一致度的總乘積)成為最大的方式，對于針對先前的計 測位置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，或者 使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，從而進(jìn)行位置對準(zhǔn)，其 中該一致度是通過基于所述鄰近的誤差分布間的極大似然推定值而確 定的。在該情況下，所述與一致度有關(guān)的評價值的計算式以上述[算式 l]表示。發(fā)明的效果根據(jù)所述本發(fā)明的方法和裝置，由于將三維形狀所存在的空間區(qū) 域分割為多個體素，并存儲各體素位置，所以即使是計測對象物很大 的情況下，也能夠?qū)?shù)據(jù)量抑制在與體素數(shù)成比例的小的數(shù)據(jù)尺寸。此外，因為在對應(yīng)于坐標(biāo)值的體素的內(nèi)部設(shè)定并儲存代表點及其 誤差分布，所以能夠表現(xiàn)體素的分辨能力以上的信息。此外，通過設(shè)定、保存表示在體素的內(nèi)部物體存在概率的概率值， 從而即使在誤差分布比代表點所屬的體素擴展的情況下，也不用找到 代表點所屬的體素，從其誤差分布再計算各體素中的物體的存在有無， 而是僅以該體素的概率值就能容易判斷，所以能夠抑制檢索時間。此外，由于根據(jù)過去的計測位置的變化推定當(dāng)前的計測位置，或 通過能夠取得當(dāng)前的計測位置的傳感器來取得，或不僅利用計測數(shù)據(jù) 的距離值還利用反射強度值，限定核對的范圍，所以能夠抑制檢索時 間。此外，由于在粗略對準(zhǔn)步驟中，對于針對先前的計測位置的環(huán)境 模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，并以與具有代表點 的體素間距離有關(guān)的評價值(例如，該距離的總和)成為最小的方式，進(jìn)行位置對準(zhǔn)，其中該代表點是與計測數(shù)據(jù)以及誤差分布鄰近的代表 點，或者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與具有代 表點的體素間距離有關(guān)的評價值(例如，該距離的總和)成為最小的 方式，進(jìn)行位置對準(zhǔn)，或者，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平 移，并以與具有代表點的體素的概率值有關(guān)的評價值(例如，該概率 值的總和)成為最大的方式，進(jìn)行位置對準(zhǔn)，其中該代表點是與計測 數(shù)據(jù)以及誤差分布鄰近的代表點，或者使針對新的計測位置的環(huán)境模 型旋轉(zhuǎn)及平移，并以與鄰近的體素所具有的概率值的差有關(guān)的評價值 (例如，該概率值的差的總和)成為最小的方式，進(jìn)行位置對準(zhǔn)，所 以能夠 一 邊防止誤差的累積， 一 邊短時間地進(jìn)行具有代表點的體素彼 此的位置對準(zhǔn)。此外，由于對于針對先前的計測位置的環(huán)境模型，使針對新的計 測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與鄰近的體素所具有的概率值的 差有關(guān)的評價值(例如，該概率差的總和)成為最小的方式，進(jìn)行位 置對準(zhǔn)，在該情況下，還附加物體不存在的信息進(jìn)行位置對準(zhǔn)，所以 能夠謀求精度的提高。接著，由于在精密對準(zhǔn)步驟中，對于針對先前的計測位置的環(huán)境 模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，或者使針對新的計測位置的環(huán)境;f莫型旋轉(zhuǎn)及平移，并以鄰近的誤差分布間距離的總和成為最小的方式，進(jìn)行位置對準(zhǔn)，所以能夠短時間地進(jìn)行誤差分布彼此 的精密的位置對準(zhǔn)。因此，可利用通過將逐次得到的計測數(shù)據(jù)綜合到本發(fā)明的環(huán)境模 型而得到的模型數(shù)據(jù)，來求出計測數(shù)據(jù)最一致的位置姿勢，而且能夠 防止誤差的累積。此外，由于當(dāng)使用本發(fā)明的環(huán)境模型時，通過在體素內(nèi)具有一個代表點從而點數(shù)減少，因此能夠使計算量降低。即，由于在本發(fā)明提 出的數(shù)據(jù)結(jié)構(gòu)是在體素內(nèi)保存 一個代表點，由于能夠使探測對應(yīng)于計 測點的模型點的計算量級為1,所以能夠?qū)⒄w的計算量級減少為O(N)。此外，雖然點數(shù)減少，但本發(fā)明的環(huán)境模型通過進(jìn)行代表點的誤 差分布和體素的分級化，能夠保持精度。進(jìn)而，原來的ICP算法對于稀疏數(shù)據(jù)輸出錯誤的結(jié)果，但本發(fā)明 的環(huán)境模型由于在體素內(nèi)具有代表點和誤差分布，所以對應(yīng)于稀疏數(shù) 據(jù)的位置對準(zhǔn)成為可能。因此，根據(jù)本發(fā)明的方法和裝置，在使用多個視點的計測數(shù)據(jù)的 三維形狀信息復(fù)原中，即使是稀疏數(shù)據(jù)也能防止誤差的累積并同時能 取得高精度的形狀。此外，由于成為重合時的比較對象的數(shù)據(jù)量減少， 所以能夠減少計算量。進(jìn)而，由于在所述模型更新步驟中，根據(jù)新輸入的被計測點的坐 標(biāo)值及其誤差分布、和已經(jīng)設(shè)定了的體素內(nèi)的代表點及其誤差分布， 通過卡爾曼濾波器取得新的代表點和誤差分布并進(jìn)行再設(shè)定，所以能 夠得到更接近真值的形狀。特別是，通過反復(fù)使用卡爾曼濾波器的模型更新步驟，從而即使 是包含誤差的數(shù)據(jù)也能通過卡爾曼濾波器的效果得到收斂到真值的高 精度的形狀。此外，通過在所述精密對準(zhǔn)步驟中，代替以與所述鄰近的誤差分 布間距離有關(guān)的評價值成為最小的方式進(jìn)行位置對準(zhǔn)，而是以與 一致 度有關(guān)的評價值(例如，該一致度的總乘積)成為最大的方式，對于 針對先前的計測位置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn) 及平移，或者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移來進(jìn)行位置 對準(zhǔn)，其中該 一致度是通過基于所述鄰近的誤差分布間的極大似然推 定值而確定的，從而，能夠考慮環(huán)境模型、計測數(shù)據(jù)雙方的誤差來進(jìn) 行位置對準(zhǔn)。本發(fā)明的其他目的以及有利的特征可從參照附圖的以下說明而明確。


圖1是在非專利文獻(xiàn)2中公開的三維激光雷達(dá)的結(jié)構(gòu)圖。圖2A是表示以距離傳感器計測的極坐標(biāo)數(shù)據(jù)和誤差的關(guān)系的圖。 圖2B表示將誤差分布作為包含在長方體中的橢圓體來近似的情況。圖3是用于執(zhí)行本發(fā)明的方法的裝置結(jié)構(gòu)圖。 圖4是表示本發(fā)明的方法的流程圖。 圖5是模型構(gòu)筑步驟的示意圖。 圖6是構(gòu)筑的環(huán)境模式的示意圖。圖7A是表示本發(fā)明的體素數(shù)據(jù)的數(shù)據(jù)構(gòu)造的圖，表示了各體素數(shù) 據(jù)的存儲器布局例。圖7B是表示本發(fā)明的體素數(shù)據(jù)的數(shù)據(jù)構(gòu)造的圖，表示了等級 (level) 2(1, 1, 0)的體素具有代表點的情況的例子。圖8是粗略對準(zhǔn)步驟S6和精密對準(zhǔn)步驟S7的數(shù)據(jù)處理流程圖。圖9是粗略對準(zhǔn)步驟S6的示意圖。圖IO是精密對準(zhǔn)步驟S7的示意圖。圖11是模型更新步驟中的數(shù)據(jù)處理流程圖。圖12是在相應(yīng)的體素內(nèi)有已經(jīng)設(shè)定了的代表點情況的示意圖。圖13表示在誤差分布相互重疊的情況下，根據(jù)兩誤差分布再設(shè)定 新的誤差分布和新的誤差分布的中心，其結(jié)果是新的代表點移動到其 他體素內(nèi)的情況。圖14是誤差分布相互重疊的情況的示意圖。圖15表示通過使用了卡爾曼濾波器的模型更新步驟得到的結(jié)果。圖16是圖15的局部放大圖。圖17表示考慮了誤差的對應(yīng)。
具體實施方式
以下參照附圖對本發(fā)明的優(yōu)選實施方式進(jìn)行說明。再有，在各圖 中，對共通的部分賦予同一附圖標(biāo)記，省略重復(fù)的說明。圖1是作為距離傳感器的一例的三維激光雷達(dá)的結(jié)構(gòu)圖。三維激 光雷達(dá)例如是在非專利文獻(xiàn)3中公開的。如該圖所示，三維激光雷達(dá)10包括雷達(dá)頭(radar head) 12和控 制器20。從激光二極管13激發(fā)的脈沖激光1在投射透鏡14被整形為平行光2，用反射鏡18a、 18b和旋轉(zhuǎn)/搖動的多角鏡15在二維方向上 進(jìn)行掃描，照射到測定對象物。從測定對象物反射的脈沖激光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是以計測位置(雷達(dá)頭設(shè)置位置)為原點的距離，以r=cxt/2的 式子求出。在這里c是光速。判定處理單元23將來自信號處理板的極坐標(biāo)數(shù)據(jù)，向以雷達(dá)頭設(shè) 置位置為原點的三維空間數(shù)據(jù)(x， y, z)轉(zhuǎn)換，進(jìn)行檢測處理。再有 在該圖中24是驅(qū)動單元。所述三維激光雷達(dá)10的計測范圍，例如是水平視場角60°、垂直 視場角30。、最大測定距離50m。此外，位置檢測精度例如是大約20cm。此外，在將計測數(shù)據(jù)以對各像素具有進(jìn)深方向的距離值的距離圖 像來表示的情況下， 一幀的計測點數(shù)為橫向166點，掃描方向是50點 時，1幀表示166x50=8300點。這種情況下幀頻例如大約是2幀/秒。在該三維激光雷達(dá)10計測的三維形狀上的計測點，是橫向上Aexr、 垂直方向上Acpxr、互相離散的點群。例如，在么0=60/166\兀/180=6.3><10-3 弧度、Aq^30/50x兀/180-10.5xlO-3弧度、產(chǎn)50m的情況下，即使是最接 近的情況，纟皮計測點的間隔也是在纟黃向大約是315mm，在垂直方向大 約是525mm。在本發(fā)明中，作為距離傳感器，例如使用上述的三維激光雷達(dá)10。 但是，距離傳感器并不限定于此，也可以使用利用視差的距離傳感器 等其他公知的距離傳感器。圖2A、圖2B是表示在距離傳感器計測的極坐標(biāo)數(shù)據(jù)和誤差的關(guān) 系的圖。如圖2A所示，將以任意的計測位置為原點的極坐標(biāo)值(r, e, (p) 作為計測結(jié)果進(jìn)行計測。根據(jù)距離傳感器的計測結(jié)果中，通常存在圖 示那樣的誤差分布。該誤差分布在誤差分布的在rs、 0S、 cps的存在概率為P (rs， es， cps) 的情況下，誤差分布是在計測的軸r、 e、 cp方向上正態(tài)分布，例如能夠以式(1)表示。這里，r、 0、 cp是來自傳感器的計測值，or、 ae、 是標(biāo)準(zhǔn)偏差，A是歸一化常數(shù)。如圖2B所示，誤差分布是通常內(nèi)包于r方向上的長切頭圓錐形(左 圖)的分布，但在遠(yuǎn)方a和b的差小。因此，能夠?qū)⒃撜`差分布作為 包含在長方體中的橢圓體而近似于安全側(cè)。[算式2]<formula>formula see original document page 21</formula>圖3是用于執(zhí)行本發(fā)明的方法的裝置結(jié)構(gòu)圖。如該圖所示，該裝 置具備數(shù)據(jù)輸入裝置32、外部存儲裝置33、內(nèi)部存儲裝置34、中央 處理裝置35以及輸出裝置36。數(shù)據(jù)輸入裝置32具有上述的距離傳感器，對存儲裝置輸入三維形 狀上的坐標(biāo)值。此外，例如并用測角儀、測距器(odometer)等，還輸 入距離傳感器的位置/姿勢或移動距離較好。再有，數(shù)據(jù)輸入裝置32還 具有鍵盤等通常的輸入單元較好。外部存儲裝置33是硬盤、軟(floppy (注冊商標(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)筑裝置是進(jìn)行后述的模型構(gòu)筑步驟的裝置，匹配裝置是進(jìn)行后述的匹配步驟的裝置，位置對準(zhǔn)裝置是進(jìn)行后述的粗略對準(zhǔn)步驟 和精密對準(zhǔn)步驟的裝置，模型更新裝置是進(jì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 (計算機)組 合在一起的也可，或是將整體一體化的裝置也可。此外， 一體地裝入 能夠自動推進(jìn)的裝置內(nèi)也可
圖4是表示本發(fā)明的方法的流程圖。
本發(fā)明的方法是用于根據(jù)三維形狀上的被計測點的坐標(biāo)值來復(fù)原 三維形狀的三維形狀數(shù)據(jù)的位置對準(zhǔn)方法，具有數(shù)據(jù)輸入步驟S1、
數(shù)據(jù)校正步驟S2、探測范圍限定步驟S3、模型構(gòu)筑步驟S4、匹配步驟 S5、粗略對準(zhǔn)步驟S6、精密對準(zhǔn)步驟S7、模型更新步驟S8以及輸出 步驟S9。
再有，在該一系列的處理中，Sl、 S2、 S3、 S5 S9每當(dāng)?shù)玫接嫓y 數(shù)據(jù)時實施，S4僅在初次得到計測數(shù)據(jù)時實施。
在數(shù)據(jù)輸入步驟S1，使用距離傳感器，對計算機的存儲裝置輸入 三維形狀上的坐標(biāo)值。此外，例如并用測角儀、測距器等還輸入距離 傳感器的位置/姿勢或移動距離較好。
再有，在該數(shù)據(jù)輸入步驟S1中，使用三維激光雷達(dá)IO,將三維形 狀上的坐標(biāo)值作為以任意的計測位置為原點的距離數(shù)據(jù)， 一 邊移動原 點一邊依次取得較好。
在作為距離傳感器使用三維激光雷達(dá)10的情況下，三維形狀上的 坐標(biāo)值是以任意的計測位置為原點的距離數(shù)據(jù)，以極坐標(biāo)值(r, e， cp) 表示。此外，各坐標(biāo)值的誤差分布根據(jù)極坐標(biāo)值(r, e, (p)以運算求 得，或預(yù)先以別的輸入單元(例如鍵盤)輸入。
在數(shù)據(jù)校正步驟S2,進(jìn)行距離數(shù)據(jù)的校正處理，提高距離數(shù)據(jù)的 精度。此外，根據(jù)極坐標(biāo)數(shù)據(jù)和測距器的數(shù)據(jù)，轉(zhuǎn)換為以任意的固定位置為原點的三維空間數(shù)據(jù)(x, y， z)也可。
在距離數(shù)據(jù)的校正處理，進(jìn)行孤立點的去除、統(tǒng)計上的處理等。 孤立點是從周圍的點孤立而存在的點，由于計測數(shù)據(jù)是以多個接近的 點構(gòu)成的，所以能夠假定孤立點是錯誤計測而去除。統(tǒng)計上的處理是 考慮計測數(shù)據(jù)包含的誤差分布，通過統(tǒng)計處理(例如平均值等)多次 的計測，進(jìn)行距離的沖交正。
進(jìn)而，成為對象的三維形狀在能夠直線近似或平面近似的情況下
進(jìn);f亍這些也可。
在探測范圍限定步驟S3，限定距離傳感器的探測范圍。 當(dāng)不限定探測范圍而對環(huán)境模型進(jìn)行計測數(shù)據(jù)的匹配處理時，有 得到多個解(被計測點)的可能性。因此，實施(l)根據(jù)過去的傳 感器位置的變化推定當(dāng)前的傳感器位置，探測傳感器位置推定結(jié)果的 鄰近，(2)使用測距器推定傳感器位置，限定探測范圍，(3)距離數(shù) 據(jù)中，不僅是距離值，也利用反射強度值來縮小探測結(jié)果等等。
圖5是對體素的分割使用八叉樹的情況下的模型構(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)僅存在單一的被計測點的方式，例如在選擇八叉樹的情況下，進(jìn) 一步八分割體素，分級地分割為多個體素。以下，將實施了一次最大 的體素6的八分割的空間區(qū)域稱為等級2的體素，將實施了 k次的空 間區(qū)域稱為等級k+l的體素。
圖6是構(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)、 riz(k)是在等級k的體素的地址號碼，L 是所要求的代表點存在的等級。[算式3](w) + 2》屋|Er"#)，…(2)圖7A、圖7B是表示本發(fā)明的體素數(shù)據(jù)的數(shù)據(jù)結(jié)構(gòu)的圖。在該圖中，圖7A是各體素數(shù)據(jù)的存儲器布局的例子。在該圖中， 箭頭表示對數(shù)據(jù)的鏈接，將對數(shù)據(jù)的指針作為值而保持。圖7B表示等級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)境^t型滿足以下全部效果A ~ C。效果A:考慮了誤差的表現(xiàn)是可能的。 效果B:需要的存儲量和計算量在一定量以下。 效果C:不僅能表示物體的存在，也能表示物體不存在。此外在圖4中，粗略對準(zhǔn)步驟S6和精密對準(zhǔn)步驟S7在匹配步驟S5之后實施。圖8是粗略對準(zhǔn)步驟S6和精密對準(zhǔn)步驟S7的數(shù)據(jù)處理流程圖， 圖9是粗略對準(zhǔn)步驟S6的示意圖，圖IO是精密對準(zhǔn)步驟S7的示意圖。在圖8中，在粗略對準(zhǔn)步驟S6，如圖9所示，對于針對先前的計 測位置的環(huán)境;f莫型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，并以 與具有代表點的體素間距離有關(guān)的評價值(例如，該距離的總和)成 為最小的方式，進(jìn)行位置對準(zhǔn)，其中該代表點是與計測數(shù)據(jù)以及誤差 分布鄰近的代表點，或者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移， 并以與具有代表點的體素間距離有關(guān)的評價值(例如，該距離的總和) 成為最小的方式，進(jìn)行位置對準(zhǔn)，或者，使新的計測數(shù)據(jù)以及誤差分 布旋轉(zhuǎn)及平移，并以與具有代表點的體素的概率值有關(guān)的評價值(例 如，該概率值的總和)成為最大的方式，進(jìn)行位置對準(zhǔn)，其中該代表 點是與計測數(shù)據(jù)以及誤差分布鄰近的代表點，或者使針對新的計領(lǐng)'J位 置的環(huán)境模型旋轉(zhuǎn)及平移，并以與鄰近的體素所具有的概率值的差有 關(guān)的評價值(例如，該概率值的差的總和)成為最小的方式，進(jì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嚴(yán)時，能夠以[算式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 26</formula>在圖8中，在精密對準(zhǔn)步驟S7，如圖IO所示那樣，對于針對先前 的計測位置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移， 或者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與鄰近的誤差 分布間距離有關(guān)的評價值(例如，該距離的總和)成為最小的方式， 進(jì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ù)的誤差分布 為SDl，誤差分布的合成函數(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, cp, \|/)變化并移動Pw而使s為最小的 最小二乘法來計算。[算式5]<formula>formula see original document page 26</formula>進(jìn)而在圖4中，模型更新步驟S8在精密對準(zhǔn)步驟S7之后實施， 更新在模型構(gòu)筑步驟S4構(gòu)筑的環(huán)境模型。圖11是模型更新步驟S8中的數(shù)據(jù)處理流程圖。如該圖所示，在 步驟ST1探測對應(yīng)于新輸入的被計測點的坐標(biāo)值的體素，在步驟ST2 相應(yīng)體素內(nèi)沒有代表點(體素是空的)的情況下，將在步驟ST3新輸 入的被計測點的坐標(biāo)值和誤差分布作為代表點的坐標(biāo)值和誤差分布而 設(shè)定(新注冊)。此外，在該步驟ST3中，新的計測位置(原點)和一皮計測點之間， 原理上物體不應(yīng)該存在。因此再設(shè)定或擦除位于新的計測位置(原點) 和:陂計測點之間的體素內(nèi)的代表點和誤差分布。圖12是在相應(yīng)體素內(nèi)有已經(jīng)設(shè)定了的代表點的情況的示意圖。在圖11的步驟ST2相應(yīng)體素內(nèi)有已經(jīng)設(shè)定了的代表點的情況下， 在步驟ST4比較新取得的誤差分布和已經(jīng)設(shè)定了的體素內(nèi)的誤差分布 (即判斷是不同點還是相同點)。在該比較，在誤差分布相互重疊的情況下(圖12的(A)),在步 驟ST5根據(jù)兩誤差分布、或根據(jù)兩誤差分布和體素內(nèi)已經(jīng)設(shè)定了的代 表點和新輸入的^皮計測點的坐標(biāo)值，再設(shè)定新的誤差分布和新的代表 點(即合成誤差分布)。此外在該比較，在誤差分布不相互重疊的情況下(圖12的(B))， 在步驟ST6、 ST7以在單一的體素內(nèi)僅存在單一的代表點的方式，進(jìn)一 步八分割該體素并分級地分割為多個體素并新注冊。分割和合成的基準(zhǔn)，例如根據(jù)誤差分布的一致度來判斷。誤差分 布的 一致度例如能夠利用馬哈拉諾比斯距離那樣的距離尺度。此外， 基于兩個誤差分布，通過統(tǒng)計上的檢驗來判定兩者是不是表示同一點 也可。在步驟ST5根據(jù)兩誤差分布再設(shè)定新的誤差分布和新的誤差分布 的中心，其結(jié)果是新的代表點向其他體素內(nèi)移動時(即，在步驟ST8， "是")，返回步驟ST2，反復(fù)進(jìn)行上述處理。再有，圖13表示在步驟ST5根據(jù)兩誤差分布、或根據(jù)兩誤差分 布和體素內(nèi)已經(jīng)設(shè)定了的代表點和新輸入的被計測點的坐標(biāo)值，再設(shè) 定新的誤差分布和新的誤差分布的中心，其結(jié)果是新的代表點向其他 體素內(nèi)移動的情況。在設(shè)定表示體素內(nèi)物體的存在概率的概率值的情況下，在模型更 新步驟S8中，對應(yīng)于體素內(nèi)的代表點和誤差分布的新注冊、或再設(shè)定、 或擦除、或在分割后的新注冊的處理，體素內(nèi)的概率值也通過統(tǒng)計處 理，進(jìn)行新注冊、或再設(shè)定、或擦除、或在分割后的新注冊。圖14是誤差分布相互重疊的情況(圖12的(A))的其他的示意 圖。在步驟ST5中，作為合成兩個代表點和誤差分布并設(shè)定新的代表 點和誤差分布的單元，能夠使用卡爾曼濾波器。例如在二維的情況下， 如該圖所示那樣，當(dāng)兩個代表點分別是x(l)、 x'(2),兩個誤差分布分別 是S(l)、 S'(2),將其合成的代表點是x(2)、誤差分布是S(2)時，計算 代表點x(2)和誤差分布S(2)的示意圖如圖14。在圖4中，在輸出步驟S9，對輸出裝置36輸出體素位置以及代表 點及其誤差分布。輸出裝置36是顯示裝置(例如CRT)的情況下，優(yōu) 選在三維圖像上立體顯示。此外，將這些數(shù)據(jù)轉(zhuǎn)送到其他的裝置(例 如控制裝置、計算機)也可，在打印機輸出也可。在輸出步驟S9,與所述自身位置一起將基于所述自身位置的所述 體素位置、代表點以及誤差分布輸出到輸出裝置中。此外，在輸出步 驟S9，將體素的代表點的位置作為三維形狀的計測值對輸出裝置36 輸出，并且將表示該計測值的可靠性或精度的指標(biāo)(例如，數(shù)值)，基 于該體素內(nèi)部的誤差分布的大小，對輸出裝置36輸出也可。進(jìn)而，在 輸出步驟S9中，將體素的代表點的位置作為三維形狀的計測值對輸出 裝置36輸出時，在該體素的內(nèi)部的誤差分布的大小(擴展)比規(guī)定的 基準(zhǔn)值大的情況下，該計測值的可靠性或精度定為比規(guī)定的基準(zhǔn)低， 該體素的所述計測值(即，該體素的代表點的位置)不對輸出裝置36 輸出也可。如圖4所示的處理的次序，在新的計測位置處，每當(dāng)?shù)玫叫碌挠?測數(shù)據(jù)時，反復(fù)進(jìn)行處理，在內(nèi)部存儲裝置34以及外部存儲裝置33 的至少任一個中存放結(jié)果。為了使處理高速化，優(yōu)選在容量允許的限 度內(nèi)盡可能的將結(jié)果存放在內(nèi)部存儲裝置34。根據(jù)所述的本發(fā)明的方法和裝置，由于將三維形狀所存在的空間 區(qū)域分割為多個體素6,并將各體素位置存儲在外部存儲裝置33，所 以即使在計測對象物大的情況下，也能將數(shù)據(jù)量抑制為與體素數(shù)成比 例的小的數(shù)據(jù)尺寸。此外，由于在對應(yīng)于坐標(biāo)值的體素6的內(nèi)部設(shè)定并存儲代表點7 及其誤差分布8，所以能夠表現(xiàn)體素的分辨能力以上的信息。概率值，從而即使在誤差分布比代表點所屬的體^擴展的情況下:'、也不用找到代表點所屬的體素，從其誤差分布再計算各體素中的物體的 存在有無，而是僅以該體素的概率值就能容易判斷，所以能夠抑制檢 索時間。此外，由于根據(jù)過去的計測位置的變化推定當(dāng)前的計測位置，或 通過能夠取得當(dāng)前的計測位置的傳感器來取得，或不僅利用計測數(shù)據(jù) 的距離值還利用反射強度值，限定核對的范圍，所以能夠抑制檢索時 間。此外，由于在粗略對準(zhǔn)步驟S6中，對于針對先前的計測位置的環(huán) 境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，并以與具有代表 點的體素間距離有關(guān)的評價值(例如，該距離的總和)成為最小的方 式，進(jìn)行位置對準(zhǔn)，其中該代表點是與計測數(shù)據(jù)以及誤差分布鄰近的 代表點，或者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與具 有代表點的體素間距離有關(guān)的評價值(例如，該距離的總和)成為最 小的方式，進(jìn)行位置對準(zhǔn)，或者，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn) 及平移，并以與具有代表點的體素的概率值有關(guān)的評價值(例如，該 概率值的總和)成為最大的方式，進(jìn)行位置對準(zhǔn)，其中該代表點是與 計須'J數(shù)據(jù)以及誤差分布鄰近的代表點，或者使針對新的計測位置的環(huán) 境模型旋轉(zhuǎn)及平移，并以與鄰近的體素所具有的概率值的差有關(guān)的評 價值(例如，該概率值的差的總和)成為最小的方式，進(jìn)行位置對準(zhǔn)， 所以能夠一邊防止誤差的累積， 一邊短時間地進(jìn)行具有代表點的體素 彼此的位置對準(zhǔn)。接著，由于在精密對準(zhǔn)步驟S7中，對于針對先前的計測位置的環(huán) 境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，或者使針對新的 計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與鄰近的誤差分布間距離有關(guān) 的評價值(例如，該距離的總和)成為最小的方式，進(jìn)行位置對準(zhǔn)， 所以能夠短時間地進(jìn)行誤差分布彼此的精密的位置對準(zhǔn)。因此，能夠利用通過將逐次得到的計測數(shù)據(jù)綜合到本發(fā)明的環(huán)境 模型而得到的模型數(shù)據(jù)，能夠求出計測數(shù)據(jù)最一致的位置姿勢，而且能夠防止誤差的累積。此外，由于當(dāng)使用本發(fā)明的環(huán)境模型時，通過在體素內(nèi)具有一個 代表點從而點數(shù)減少，因此能夠使計算量降低。此外，雖然點數(shù)減少， 但本發(fā)明的環(huán)境模型通過進(jìn)行代表點的誤差分布和體素的分級化，能 夠保持精度。進(jìn)而，原來的ICP算法對于稀疏數(shù)據(jù)輸出錯誤的結(jié)果，但本發(fā)明 的環(huán)境模型由于在體素內(nèi)具有代表點和誤差分布，所以對應(yīng)于稀疏數(shù) 據(jù)的位置對準(zhǔn)成為可能。因此，根據(jù)本發(fā)明的方法和裝置，在使用多個視點的計測數(shù)據(jù)的 三維形狀信息復(fù)原中，即使是稀疏數(shù)據(jù)也能防止誤差的累積并同時取 得高精度的形狀。此外，由于成為重合時的比較對象的數(shù)據(jù)量減少， 所以能夠減少計算量。此外，由于在模型構(gòu)筑步驟S4中，在將最大的體素9設(shè)定為相當(dāng) 于必要最小限度的分辨能力的大小，而且單一的體素9內(nèi)存在多個被 計測點的情況下，以在單一的體素內(nèi)僅存在單一的被計測點的方式， 進(jìn)一步將該體素八分割，分級地分割成多個體素，所以能夠?qū)?shù)據(jù)量 抑制為小的數(shù)據(jù)尺寸的同時，能夠使用分割后的體素和代表點進(jìn)一步 提高分辨率。特別是，將三維形狀上的多個坐標(biāo)值作為以多個計測位置為原點 的距離數(shù)據(jù)而取得，通過將該距離數(shù)據(jù)的坐標(biāo)值作為所述代表點的坐 標(biāo)值，將距離數(shù)據(jù)的坐標(biāo)值的計測誤差作為代表點的誤差分布，從而 能夠使用正確的坐標(biāo)值和誤差分布在統(tǒng)計上綜合多次的計測，能夠愈 發(fā)提高精度。此外，作為在原點和^皮計測點之間不存在物體的，通過再設(shè)定或 擦除位于其間的體素內(nèi)的代表點和誤差分布，能夠去除錯誤計測數(shù)據(jù) 的影響。此外，探測對應(yīng)于新輸入的#_計測點的坐標(biāo)值的體素，在該體素 內(nèi)沒有代表點的情況下，通過將所述坐標(biāo)值和誤差分布設(shè)定為代表點 的坐標(biāo)值和誤差分布，能夠容易地設(shè)定代表點的坐標(biāo)值和誤差分布。進(jìn)而，在所述體素內(nèi)有已經(jīng)設(shè)定了的代表點的情況下，比較新取 得的誤差分布和已經(jīng)設(shè)定了的體素內(nèi)的誤差分布，在誤差分布相互重 疊的情況下，根據(jù)兩誤差分布、或根據(jù)兩誤差分布和體素內(nèi)已經(jīng)設(shè)定了的代表點和新輸入的被計測點的坐標(biāo)值，再設(shè)定新的誤差分布和新 的代表點，在誤差分布不相互重疊的情況下，以在單一的體素內(nèi)僅存 在單一的代表點的方式，將該體素進(jìn)一步分割并分級地分割為多個體 素。由此，能夠回避誤差的累積并同時收斂到高精度的形狀。因此，根據(jù)本發(fā)明的方法和裝置，具有將包含誤差的距離數(shù)據(jù)校 正為正確的信息的功能，并且通過反復(fù)該功能，從而對于長時間的計 測會收斂到高精度的形狀。而且，本發(fā)明的方法，由于是以新的計測點來更新對應(yīng)于各體素6的代表點7及其誤差分布8的處理，所以計 算量小。此外，由于運算不對周圍的體素施加影響而封閉在體素內(nèi)， 所以能夠高速處理。此外，計測數(shù)據(jù)能夠逐次綜合為最大體素具有必 要最小限度的分辨能力的體素結(jié)構(gòu)，存儲量不會大幅超過固定量。 對使用了卡爾曼濾波器的模型更新步驟進(jìn)行詳細(xì)說明。 在使用了卡爾曼濾波器的模型更新步驟的情況下，根據(jù)新輸入的 -故計測點的坐標(biāo)值及其誤差分布、和已經(jīng)設(shè)定了的體素內(nèi)的代表點及 其誤差分布，通過卡爾曼濾波器取得新的代表點和誤差分布并進(jìn)行再 設(shè)定。以各模型點群的位置m(i)為狀態(tài)量，基于距離傳感器的計測點的 位置，以下式[算式6]表現(xiàn)模型。再有，在本實施例，m(i)是體素內(nèi)部 的代表點(以下相同)。[算式6]L(j) = hm(Rr，tr，m(i)) + vL(j)=RJIV!(m(i) - tr) - ts) + vL(j) (j = l，... ，N)在[算式6]中，L(j)是根據(jù)距離傳感器的計測位置。例如L(j)是在距離傳感器的傳 感器坐標(biāo)系中三維LRF (激光測距儀)的計測點j (j=l, ...， N)的位 置L(j)=(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、 0z分別表示繞x軸、y軸、z軸的旋轉(zhuǎn)角(以下相同)。tr是表示裝載了距離傳感器的移動體(例如移動機器人)的相對于世界坐標(biāo)系的位置的平移矢量t產(chǎn)(x, y， z)。VL(i)是加入到距離傳感器的計測值L(j)的觀測噪聲。 Rs是傳感器坐標(biāo)系的對移動體坐標(biāo)系的旋轉(zhuǎn)矩陣R^R (ex, ey,ez)。ts是表示傳感器坐標(biāo)系的對移動體坐標(biāo)系的位置的平移矢量t^(X,y， z)。測定對象物是靜止的，將測定對象物的位置tr、姿勢Rr對環(huán)境模 型固定。使根據(jù)距離傳感器的計測點群和環(huán)境模型點群上的點i(即代表點) 相對應(yīng)。對進(jìn)行該對應(yīng)的模型點群上的點i通過下式(4)進(jìn)行更新。 再有，可僅對與根據(jù)距離傳感器的計測點群進(jìn)行了對應(yīng)的模型點群上的代表點m (i)通過如下的[算式7]進(jìn)行更新。 [算式7]Kmk(" 2W氛肌k(U)H』)"2Lk(j)" m，k(i) = mk,k—Ji) +Kmk(i)(Lk(j) - hJRrk，trk，mk,H(i))) 2，mk(i)- 2喊H(i) —Kmk(i)Hmk(j〉2mk,k—Ji)在[算式7]中，下標(biāo)k表示是在離散時刻k的值。關(guān)于mk(i)， m'k(i)表示mk(i)的更新值(事后推定值)，mk, w(i)表 示基于m'w(i)的mk(i)的預(yù)測值(事前推定值)。再有因為環(huán)境(測定 對象物)靜止，所以mk, m k-i(i)。Smk(i)是體素內(nèi)部的代表點mk(i)的誤差協(xié)方差矩陣(即所述誤差分 布)。此外，關(guān)于Smk(i), 2:'mk(i)表示！^k(i)的更新值(事后推定值),Smk,W(i)表示基于S'mw(i)的2mk(i)的預(yù)測值(事前推定值)。將在傳感器坐標(biāo)系中將三維LRF的計測點j(j=l,N)的位置以L(j)表示，將其誤差協(xié)方差矩陣以SL(j )表示。在這里N是在三維LRF得到的計測點的總 數(shù)。作為三維LRF的誤差模型假定與計測距離沒有關(guān)系的一定的正態(tài) 分布。將對傳感器坐標(biāo)系的x軸方向照射激光的情況下的誤差協(xié)方差 矩陣作為Ss。對應(yīng)激光的照射方向誤差分布也改變姿勢。&(0是將對 基準(zhǔn)方向的激光照射方向使用旋轉(zhuǎn)矩陣Rl(O表示為 SL(j)二RL(j)SsRLt(j)。計測點j的在世界坐標(biāo)系的位置z(j)、及其誤差協(xié) 方差矩陣2z(j)能夠分別以z(j)=Rr(RsL(j)+ts)+tr、 Sz(JhRrRsSiXJ)RstRrt來 表示。Kmk(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(O的觀測系模型的雅可比矩陣，以下面的[算式 8〗表示。[算式8]根據(jù)卡爾曼濾波器的更新過程，在得到了環(huán)境地圖的模型點群的 各點(體素的代表點)的位置和誤差協(xié)方差矩陣的更新值m'k(i)、 Smk(i) 的階段，以下面的次序進(jìn)行環(huán)境模型的更新。(1 )將這些更新值m'k(i)、 2mk(i)作為新的代表點、誤差分布再設(shè)定。(2) 所述(1)的結(jié)果是，代表點的位置移動到別的體素內(nèi)的情況下， 當(dāng)移動目的地的體素沒有保持代表點時，使移動目的地的體素保持移 動后的代表點及其誤差協(xié)方差矩陣，從移動起始地的體素去除代表點 等。當(dāng)移動目的地的體素已經(jīng)保持有代表點時，在兩個代表點中，判 斷其兩誤差分布是否重疊(與所述ST4中的判斷相同)。之后的處理與 圖11的ST4以后的處理相同即可。(3) 關(guān)于沒有進(jìn)行與模型點群上的代表點m(i)對應(yīng)的根據(jù)距離傳感器 的計測點，在包含該計測點的體素不具有代表點的情況下，將計測點及其誤差分布作為該體素的代表點和誤差分布追加并保持。如果體素 內(nèi)代表點已經(jīng)存在的情況下，以包含位于體素內(nèi)的沒有進(jìn)行對應(yīng)的其 他的多個計測點，既存的代表點和各計測點被包含在完全不同體素的 方式，分割體素并在此基礎(chǔ)上使代表點等繼承到分割后的體素。通過反復(fù)所述的使用了卡爾曼濾波器的模型更新步驟，從而漸漸 地體素內(nèi)的誤差協(xié)方差矩陣(即誤差分布)的范圍變小，并且體素變 得容易分割。通過分割體素，還能夠表現(xiàn)初期體素的大小以下的變化。圖15表示通過使用了卡爾曼濾波器的模型更新步驟得到的結(jié)果。 圖16是圖15的局部放大圖。在這些圖中，以初期的體素的一個邊的 長度為100cm，允許6分割以下的再分割數(shù)。在對象存在的區(qū)域，反 復(fù)體素的再分割，其結(jié)果是精度良好地表現(xiàn)計測對象。可知在對象不 存在的區(qū)域不進(jìn)行體素的再分割，能夠以所需的充分的數(shù)據(jù)量來表現(xiàn) 環(huán)境。此外，各體素內(nèi)的代表點的誤差分布也小，能夠以高精度表現(xiàn) 環(huán)境地圖。這樣，即使是包含誤差的數(shù)據(jù)，也能夠通過卡爾曼濾波器 的效果，得到收斂到真值的結(jié)果。進(jìn)而，在該方法通過使計測數(shù)據(jù)數(shù) 增加，從而標(biāo)準(zhǔn)偏差變小，能夠期待精度的進(jìn)一步提高。此外，由于測定對象物的位置/姿勢固定，所以能夠與測定對象物 的位置/姿勢獨立來進(jìn)行更新。再有，通過僅對與根據(jù)距離傳感器的計 測點群進(jìn)行了對應(yīng)的模型點群上的代表點m(i),進(jìn)行根據(jù)所述的卡爾 曼濾波器的更新，從而能夠大幅削減計算成本。在所述精密對準(zhǔn)步驟中，也可代替以所述鄰近的誤差分布間距離 的總和成為最小的方式進(jìn)行位置對準(zhǔn)，而是以與 一 致度有關(guān)的評價值 (例如，該一致度的總乘積)成為最大的方式，對于針對先前的計測 位置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，或者使 針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，從而進(jìn)行位置對準(zhǔn)，其中 該一致度是通過基于所述鄰近的誤差分布間的極大似然推定值而確定 的。對該情況進(jìn)行詳細(xì)地說明。由于對作為環(huán)境地圖(環(huán)境模型)的模型點群、以及傳感器的計 測點群雙方考慮了誤差模型，所以對所述與一致度有關(guān)的評價值(例 如該一致度的總乘積)的計算式(以下叫做評價函數(shù))也能夠引入雙 方的誤差。在本實施例的情況下，不是單使最近的點對應(yīng)，而是通過引入似然的概念，從而對評價函數(shù)附加環(huán)境地圖上的誤差分布、以及 計測點的誤差分布，在當(dāng)前時刻的環(huán)境地圖中，確定在各計測點的出 現(xiàn)概率成為最大的情況下評價函數(shù)也成為最大。具體地，假定為與計測點j對應(yīng)的環(huán)境地圖上的點卜p(j)的位置， 符合平均值(代表點)m(i)、誤差協(xié)方差矩陣Sm(i)的正態(tài)分布，在此基礎(chǔ)上在三維LRF計測的結(jié)果是，將得到L(j)的計測數(shù)據(jù)的概率值Pr (L(j)lm(i), Zm(i))作為點i和點j的評價函數(shù)EM(i， j),以其總乘積 為最大的方式以下面的[算式9]來確定評價函數(shù)。[算式9]<formula>formula see original document page 35</formula>其中，co(j)在模型點群中有與計測點j對應(yīng)的點存在的情況下為1， 在除此之外的情況下為0。這里，將Pr(L(j)lq)作為表示在環(huán)境地圖的點在q的位置的情況下 能夠得到L(j)的計測數(shù)據(jù)的概率值，將Pr(qlm(i)， SJi))作為在假定 符合平均值(代表點)m(i)、誤差協(xié)方差矩陣2m(i)的正態(tài)分布之上表 示環(huán)境地圖的點在q的位置的概率值時，[算式IO]成立。[算式10]<formula>formula see original document page 35</formula> 當(dāng)假定Pr (q|m(i), Sm(i))為正態(tài)分布時，成為下面的[算式ll][算式11]Pr(q|m(!.)，Sw(/))=exp(-—(q — m(i" Em(i)—' (q - m(i)))另一方面，以z(j)置換L(j), Pr(L(j)iq)能夠以下面的[算式12]近似。 [算式12]Pr(L(川q)sH 1<-exp(-4(z(j) - q" ，-1 (z(i) - q))#》①l 2這里，Zk(j)依賴于裝載了距離傳感器的移動體的位置tr、姿勢Rr。實際上，由于從三維LRF的傳感器坐標(biāo)系中心看到的q的朝向和計測 點L(j)的朝向與圖17所示的不同，所以誤差協(xié)方差矩陣2UJ)也需要配 合q的朝向旋轉(zhuǎn)轉(zhuǎn)換，但由于從對應(yīng)的環(huán)境地圖上的點i遠(yuǎn)離的q的存 在概率低，所以能夠以充分的精度近似。由此，Pr(L(j)lm(i), 、(i))能 夠以下面的[算式13]表示。[算式13]<formula>formula see original document page 37</formula>通過簡單的計算，得到下面的[算式M][算式14]<formula>formula see original document page 37</formula>其中，a(j)、卩(j)能夠以下面的[算式15]來表示, [算式15〗<formula>formula see original document page 38</formula>因此，表示模型點群的點i和計測點群的點j的對應(yīng)的一致度的評 價函數(shù)EM(i, j),在平均值m(i)、誤差協(xié)方差矩陣Sm(i)+ 2:z(j)的正態(tài) 分布中，能夠近似為得到z(j)的概率值。通過使用該評價函數(shù)，能夠進(jìn) 行考慮了環(huán)境地圖、計測數(shù)據(jù)雙方的對應(yīng)。關(guān)于計測點和環(huán)境地圖(即，環(huán)境模型)的對應(yīng)進(jìn)行補充說明。 在上述實施例，由于使用考慮了誤差分布的統(tǒng)計上的評價函數(shù)，所以 不求出評價函數(shù)的值就不能夠確定對應(yīng)點。因此，在環(huán)境地圖上的才莫 型點群中預(yù)先縮小對應(yīng)的候補，從該候補中基于評價函數(shù)的值求出對 應(yīng)點。具體地，能夠如以下方式確定。(1) 求出與作為對象的計測點j的誤差協(xié)方差矩陣^(0的范圍(例如 標(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)計上的明確的判斷尺度，所以不會 有即使在認(rèn)為對應(yīng)點不存在的情況下還勉強地進(jìn)行對應(yīng)的情況。再有， 在沒有對應(yīng)點的情況下，解釋為成為對象的計測點是相當(dāng)于至此未計 測的部分的點，追加到環(huán)境地圖。作為實施方式，對三維形狀數(shù)據(jù)的位置對準(zhǔn)方法和裝置進(jìn)行了說明，但通過將二維形狀作為三維形狀的特殊情況來看，也能夠作為二 維形狀數(shù)據(jù)的位置對準(zhǔn)方法和裝置的方式來實施。此外，在所述輸出步驟中，不全部輸出所述體素位置、代表點以 及誤差分布也可，例如在全部沒有它們也能把握三維形狀的情況、或 需要它們中的一個或兩個的情況等情況下，可將所述體素位置、代表 點以及誤差分布的至少任一個輸出到輸出裝置中。再有，本發(fā)明并不局限于上述實施方式，當(dāng)然能夠在不脫離本發(fā) 明的主旨的范圍內(nèi)進(jìn)行各種變更。
權(quán)利要求
1.一種三維形狀數(shù)據(jù)的位置對準(zhǔn)方法，用于從多個計測位置來計測靜止的三維形狀，綜合其距離數(shù)據(jù)以進(jìn)行位置對準(zhǔn)，其特征在于，實施下述步驟數(shù)據(jù)輸入步驟，在新的計測位置處將三維形狀上的坐標(biāo)值輸入到計算機中；模型構(gòu)筑步驟，構(gòu)筑環(huán)境模型，該環(huán)境模型將所述三維形狀所存在的空間區(qū)域分割為由邊界表面相互正交的長方體構(gòu)成的多個體素，并存儲各體素位置；以及匹配步驟，在對應(yīng)于所述坐標(biāo)值的體素的內(nèi)部設(shè)定代表點及其誤差分布并存儲起來，所述方法還具有下述步驟精密對準(zhǔn)步驟，對于針對先前的計測位置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，或者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以鄰近的誤差分布間距離的總和成為最小的方式，進(jìn)行位置對準(zhǔn)；以及輸出步驟，將所述體素位置、代表點以及誤差分布輸出到輸出裝置中。
2. 根據(jù)權(quán)利要求1所述的三維形狀數(shù)據(jù)的位置對準(zhǔn)方法，其特征 在于，在所述匹配步驟中，在體素內(nèi)部除了代表點及其誤差分布之外， 還設(shè)定并存儲表示在體素內(nèi)物體存在概率的概率值。
3. 根據(jù)權(quán)利要求1所述的三維形狀數(shù)據(jù)的位置對準(zhǔn)方法，其特征 在于，在所述精密對準(zhǔn)步驟之前具有粗略對準(zhǔn)步驟，對于針對先前 的計測位置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移， 并以具有代表點的體素間距離的總和成為最小的方式，進(jìn)行位置對準(zhǔn)， 其中該代表點是與計觀'J數(shù)據(jù)以及誤差分布鄰近的代表點，或者使針對 新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以具有代表點的體素間距離 的總和成為最小的方式，進(jìn)行位置對準(zhǔn)。
4. 根據(jù)權(quán)利要求1所述的三維形狀數(shù)據(jù)的位置對準(zhǔn)方法，其特征 在于，在所述精密對準(zhǔn)步驟之前具有粗略對準(zhǔn)步驟，對于針對先前 的計測位置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移， 并以具有代表點的體素的概率值的總和成為最大的方式，進(jìn)行位置對準(zhǔn)，其中該代表點是與計測數(shù)據(jù)以及誤差分布鄰近的代表點，或者使 針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以鄰近的體素所具有的 概率值的差的總和成為最小的方式，進(jìn)行位置對準(zhǔn)。
5. 根據(jù)權(quán)利要求1所述的三維形狀數(shù)據(jù)的位置對準(zhǔn)方法，其特征 在于，在所述數(shù)據(jù)輸入步驟之后具有探測范圍限定步驟，根據(jù)過去 的計測位置的變化推定當(dāng)前的計測位置，或通過能夠取得當(dāng)前的計測 位置的傳感器來取得，或不僅利用計測數(shù)據(jù)的距離值還利用反射強度 值來限定核對的范圍。
6. 根據(jù)權(quán)利要求1所述的三維形狀數(shù)據(jù)的位置對準(zhǔn)方法，其特征 在于，在所述精密對準(zhǔn)步驟中，將誤差分布交叉的情況作為同一計測 點，將根據(jù)分布的一致度求得的加權(quán)乘以該情況的距離值，計算誤差 分布間的距離。
7. 根據(jù)權(quán)利要求1所述的三維形狀數(shù)據(jù)的位置對準(zhǔn)方法，其特征 在于，在所述模型構(gòu)筑步驟中，在將最大的體素設(shè)定為相當(dāng)于必要最 小限度的分辨能力的大小，而且單 一 的體素內(nèi)存在多個被計測點的情 況下，以在單一的體素內(nèi)僅存在單一的被計測點的方式，將該體素進(jìn) 一步分割并分級地分割成多個體素。
8. 根據(jù)權(quán)利要求1所述的三維形狀數(shù)據(jù)的位置對準(zhǔn)方法，其特征 在于，在所述精密對準(zhǔn)步驟之后具有更新所述環(huán)境模型的模型更新步 驟，在該模型更新步驟中，探測對應(yīng)于新輸入的被計測點的坐標(biāo)值的 體素，作為在原點和被計測點之間不存在物體的，再設(shè)定或擦除位于其 間的體素內(nèi)的代表點和誤差分布。
9. 根據(jù)權(quán)利要求1所述的三維形狀數(shù)據(jù)的位置對準(zhǔn)方法，其特征 在于，在所述精密對準(zhǔn)步驟之后具有更新所述環(huán)境模型的模型更新步 驟，在該模型更新步驟中，探測對應(yīng)于新輸入的被計測點的坐標(biāo)值的 體素，在該體素內(nèi)沒有代表點的情況下，將所述坐標(biāo)值和誤差分布設(shè)定 為代表點的坐標(biāo)值和誤差分布。
10. 根據(jù)權(quán)利要求1所述的三維形狀數(shù)據(jù)的位置對準(zhǔn)方法，其特征 在于，在所述精密對準(zhǔn)步驟之后具有更新所述環(huán)境模型的模型更新步 驟，在該才莫型更新步驟中，探測對應(yīng)于新輸入的被計測點的坐標(biāo)值的體素，在所述體素內(nèi)有已經(jīng)設(shè)定了的代表點的情況下，比較新取得的誤 差分布和已經(jīng)設(shè)定了的體素內(nèi)的誤差分布，在誤差分布相互重疊的情況下，根據(jù)兩誤差分布再設(shè)定新的誤差 分布和新的代表點，在誤差分布不相互重疊的情況下，以在單一的體素內(nèi)僅存在單一 的代表點的方式，將該體素進(jìn)一步分割并分級地分割為多個體素。
11. 根據(jù)權(quán)利要求10所述的三維形狀數(shù)據(jù)的位置對準(zhǔn)方法，其特 征在于，在所述模型更新步驟中，比較所述新取得的誤差分布和所述 已經(jīng)設(shè)定了的體素內(nèi)的誤差分布，在誤差分布相互重疊的情況下，根 據(jù)兩誤差分布、或根據(jù)兩誤差分布和體素內(nèi)已經(jīng)設(shè)定了的代表點和新 輸入的被計測點的坐標(biāo)值，再設(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)僅存在單 一 的代表點的方式，將該體素進(jìn)一 步分割并分級地分割 為多個體素。
12. 根據(jù)權(quán)利要求1所述的三維形狀數(shù)據(jù)的位置對準(zhǔn)方法，其特征在于，在所述精密對準(zhǔn)步驟之后具有更新所述環(huán)境模型的模型更新步 驟，在該模型更新步驟中，根據(jù)新輸入的被計測點的坐標(biāo)值及誤差分 布、和已經(jīng)設(shè)定了的體素內(nèi)的代表點及其誤差分布，通過卡爾曼濾波 器取得新的代表點和誤差分布并進(jìn)行再設(shè)定。
13. 根據(jù)權(quán)利要求1所述的三維形狀數(shù)據(jù)的位置對準(zhǔn)方法，其特征 在于，在所述精密對準(zhǔn)步驟中，代替以所述鄰近的誤差分布間距離的 總和成為最小的方式進(jìn)行位置對準(zhǔn)，而是以與 一 致度有關(guān)的評價值成 為最大的方式，對于針對先前的計測位置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，或者使針對新的計測位置的環(huán)境模型旋 轉(zhuǎn)及平移，從而進(jìn)行位置對準(zhǔn)，其中該一致度是通過基于所述鄰近的 誤差分布間的極大似然推定值而確定的。
14. 根據(jù)權(quán)利要求13所述的三維形狀數(shù)據(jù)的位置對準(zhǔn)方法，其特 征在于，所述與一致度有關(guān)的評價值的計算式以下面的[算式16]表示，[算式16]在該式中，計測點j和環(huán)境模型上的代表點i對應(yīng)起來，能得到該 計測點j的計測數(shù)據(jù)的概率為EM (i, j), (D(j)在環(huán)境^t型中存在與計 測點j對應(yīng)起來的代表點的情況下是1,在除此之外的情況下為0。
15. —種三維形狀數(shù)據(jù)的位置對準(zhǔn)裝置，用于從多個計測位置來計 測靜止的三維形狀，綜合其距離數(shù)據(jù)以進(jìn)行位置對準(zhǔn)，其特征在于， 具備數(shù)據(jù)輸入裝置，將三維形狀上的坐標(biāo)值輸入到計算機中； 模型構(gòu)筑裝置，構(gòu)筑環(huán)境模型，該環(huán)境模型將所述三維形狀所存在的空間區(qū)域分割為由邊界表面相互正交的長方體構(gòu)成的多個體素，并存儲各體素位置；匹配裝置，在對應(yīng)于所述坐標(biāo)值的體素的內(nèi)部設(shè)定代表點及其誤 差分布并存儲起來；以及數(shù)據(jù)傳輸裝置，將所述體素位置、代表點以及誤差分布輸出到輸 出裝置中，對于針對先前的計測位置的環(huán)境模型，使針對新的計測位置的環(huán) 境模型旋轉(zhuǎn)及平移，并以鄰近的誤差分布間距離的總和成為最小的方 式，進(jìn)行位置對準(zhǔn)。
16. —種三維形狀數(shù)據(jù)的位置對準(zhǔn)方法，用于從多個計測位置來計 測靜止的三維形狀，綜合其距離數(shù)據(jù)以進(jìn)行位置對準(zhǔn)，其特征在于， 實施下述步驟數(shù)據(jù)輸入步驟，在新的計測位置處將三維形狀上的坐標(biāo)值輸入到計算機中；模型構(gòu)筑步驟，構(gòu)筑環(huán)境模型，該環(huán)境模型將所述三維形狀所存 在的空間區(qū)域分割為由邊界表面相互正交的長方體構(gòu)成的多個體素，并存儲各體素位置；以及匹配步驟，在對應(yīng)于所述坐標(biāo)值的體素的內(nèi)部設(shè)定代表點及其誤 差分布并存儲起來，所述方法還具有下述步驟精密對準(zhǔn)步驟，對于針對先前的計測位置的環(huán)境模型，使新的計 測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，或者使針對新的計測位置的環(huán)境模 型旋轉(zhuǎn)及平移，并以與鄰近的誤差分布間距離有關(guān)的評價值成為最小的方式，進(jìn)行位置對準(zhǔn)；以及輸出步驟，將所述體素位置、代表點以及誤差分布的至少任一個 輸出到輸出裝置中。
17. 根據(jù)權(quán)利要求16所述的三維形狀數(shù)據(jù)的位置對準(zhǔn)方法，其特 征在于，在所述精密對準(zhǔn)步驟之前具有粗略對準(zhǔn)步驟，對于針對先 前的計測位置的環(huán)境模型，使新的計效'J數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移， 并以與具有代表點的體素間距離有關(guān)的評價值成為最小的方式，進(jìn)行 位置對準(zhǔn)，其中該代表點是與計測數(shù)據(jù)以及誤差分布鄰近的代表點， 或者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與具有代表點 的體素間距離有關(guān)的評價值成為最小的方式，進(jìn)行位置對準(zhǔn)。
18. 根據(jù)權(quán)利要求16所述的三維形狀數(shù)據(jù)的位置對準(zhǔn)方法，其特 征在于，在所述精密對準(zhǔn)步驟之前具有粗略對準(zhǔn)步驟，對于針對先 前的計測位置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移， 并以與具有代表點的體素的概率值有關(guān)的評價值成為最大的方式，進(jìn) 行位置對準(zhǔn)，其中該代表點是與計測數(shù)據(jù)以及誤差分布鄰近的代表點， 或者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以與鄰近的體素 所具有的概率值的差有關(guān)的評價值成為最小的方式，進(jìn)行位置對準(zhǔn)。
19. 一種三維形狀數(shù)據(jù)的位置對準(zhǔn)裝置，用于從多個計測位置來計 測靜止的三維形狀，綜合其距離數(shù)據(jù)以進(jìn)行位置對準(zhǔn)，其特征在于， 具備數(shù)據(jù)輸入裝置，將三維形狀上的坐標(biāo)值輸入到計算機中；模型構(gòu)筑裝置，構(gòu)筑環(huán)境模型，該環(huán)境模型將所述三維形狀所存 在的空間區(qū)域分割為由邊界表面相互正交的長方體構(gòu)成的多個體素， 并存儲各體素位置；匹配裝置，在對應(yīng)于所述坐標(biāo)值的體素的內(nèi)部設(shè)定代表點及其誤 差分布并存儲起來；以及數(shù)據(jù)傳輸裝置，將所述體素位置、代表點以及誤差分布的至少任 一個輸出到輸出裝置中，對于針對先前的計測位置的環(huán)境模型，使針對新的計測位置的環(huán) 境模型旋轉(zhuǎn)及平移，并以與鄰近的誤差分布間距離有關(guān)的評價值成為 最小的方式，進(jìn)行位置對準(zhǔn)。
全文摘要
本發(fā)明實施下述步驟數(shù)據(jù)輸入步驟(S1)，將三維形狀上的坐標(biāo)值輸入到計算機中；模型構(gòu)筑步驟(S4)，構(gòu)筑環(huán)境模型，該環(huán)境模型將三維形狀所存在的空間區(qū)域分割為由邊界表面相互正交的立方體構(gòu)成的多個體素，并存儲各體素位置；以及匹配步驟(S5)，在對應(yīng)于坐標(biāo)值的體素的內(nèi)部設(shè)定代表點及其誤差分布并存儲起來。還實施精密對準(zhǔn)步驟(S7)，對于針對先前的計測位置的環(huán)境模型，使新的計測數(shù)據(jù)以及誤差分布旋轉(zhuǎn)及平移，或者使針對新的計測位置的環(huán)境模型旋轉(zhuǎn)及平移，并以鄰近的誤差分布間距離的總和成為最小的方式，進(jìn)行位置對準(zhǔn)。并將所述體素位置、代表點以及誤差分布的至少任一個輸出到輸出裝置中。
文檔編號G01S17/89GK101331381SQ20068004747
公開日2008年12月24日 申請日期2006年12月15日 優(yōu)先權(quán)日2005年12月16日
發(fā)明者寺田英雄, 林俊寬, 河野幸弘 申請人:株式會社Ihi