專利名稱：用于測定物體的空間位置和取向的系統(tǒng)的制作方法
背景技術(shù)：
本發(fā)明一般地涉及一種用于測定物體或?qū)ο蟮目臻g位置和角度取向(即姿態(tài))的系統(tǒng)。
如現(xiàn)有技術(shù)中已經(jīng)知道的那樣，有些系統(tǒng)可測定物體(或?qū)ο?的空間位置和角度取向。一種這樣的系統(tǒng)包括無源式回射器，作為附加到物體上的點(diǎn)標(biāo)記或測量用靶，而第二種系統(tǒng)包括有源輻射源，作為附加的點(diǎn)標(biāo)記。這兩種技術(shù)都是通過將高對比度的標(biāo)記圖像投射到彼此隔開的傳感器上，并使用數(shù)學(xué)處理測定每一個點(diǎn)標(biāo)記的三維坐標(biāo)來進(jìn)行操作的。然后，將這些三維坐標(biāo)用作離散點(diǎn)，或者如果它們的幾何排列是已知的，則可以考慮為一套，這導(dǎo)致了物體在空間中有關(guān)三維坐標(biāo)系統(tǒng)的位置和角度取向的測定(即，6個自由度x，y和z以及間距、偏轉(zhuǎn)和旋轉(zhuǎn)角度取向)，其中，該坐標(biāo)系中心位于空間中的預(yù)定點(diǎn)上，典型地，在關(guān)于傳感器固定的點(diǎn)上。
測定物體的空間位置和矢量角或角度取向有幾種用途。例如，當(dāng)定點(diǎn)裝置的終端處于和標(biāo)記相關(guān)的已知位置上時，定點(diǎn)裝置能認(rèn)出物體。這種定點(diǎn)裝置可以在逆向工程應(yīng)用中用作手持?jǐn)?shù)字化定點(diǎn)。操作員將該定點(diǎn)裝置移至一制造的部件上的各種已知位置，并從對測定的定點(diǎn)裝置的終端位置進(jìn)行分析而測定制造過程的精確度。這種應(yīng)用需要高度精確的系統(tǒng)。
在另外一種應(yīng)用，例如圖像引導(dǎo)的外科手術(shù)中，跟蹤關(guān)于該病人的手術(shù)器具狀態(tài)。某些手術(shù)器械具有粘貼到它們上的標(biāo)記。這種信息可以用于允許外科醫(yī)生看見器械在MR或CT掃描器上指出的地方，以及在手術(shù)器械的終端后邊的東西。這種應(yīng)用也需要高度精確的系統(tǒng)。
在一種發(fā)射標(biāo)記(即有源標(biāo)記)系統(tǒng)中，使用多個電荷耦合裝置(CCD)傳感器測量由標(biāo)記發(fā)射的能量。在每一個傳感器周期為一個點(diǎn)標(biāo)記供給能量，以發(fā)射紅外線能量。在每一個傳感器周期中，集中在傳感器上的發(fā)射的能量被收集(即，結(jié)合)并移動到傳感器處理電路。為了測定標(biāo)記的3D位置，必須在至少三個傳感器軸上測量到標(biāo)記(即，包括至少3個正交平面)。對于一種使用發(fā)射標(biāo)記的系統(tǒng)有許多的優(yōu)點(diǎn)，包括有高度反差圖像產(chǎn)生在傳感器上，控制每一個標(biāo)記的激勵，以提供正確和自動的標(biāo)記識別，以及使用高速線性傳感器的能力。高速線性傳感器相對比較昂貴，并且在單個傳感器周期中只能跟蹤一個標(biāo)記。
在一種回射標(biāo)記(即無源標(biāo)記)系統(tǒng)中，給予能量源能量，以沿回射標(biāo)記的一般取向發(fā)射紅外線能量。然后，使用多個CCD傳感器，以測量由標(biāo)記反射的能量。在每一個傳感器周期中，將集中在傳感器上的反射能量收集(即，集中)到一起，并轉(zhuǎn)移到傳感器處理電路。為了測定標(biāo)記的3D位置，必須在至少三個傳感器軸上測量標(biāo)記(即，包括最小量的3個正交平面)。對于回射標(biāo)記系統(tǒng)具有許多優(yōu)點(diǎn)，包括利用無線電標(biāo)記，以及能夠使用不昂貴的低速分區(qū)陣列傳感器。但是，這些系統(tǒng)遇到與有效識別標(biāo)記有關(guān)的問題。
希望使用一種成本低廉的分區(qū)陣列傳感器，它能夠在單個傳感器周期中跟蹤多個標(biāo)記。如在現(xiàn)有技術(shù)中已知的，有一些系統(tǒng)利用單個分區(qū)陣列傳感器和不昂貴的部件。DeMenthon(專利號5，227，985)揭示了一種系統(tǒng)，它使用單個傳感器和矩陣技術(shù)來測定物體的姿勢。這種系統(tǒng)限于非共面標(biāo)記，并且建立在射影法基礎(chǔ)上，以從2D圖像中提取6D信息。這種方法對于醫(yī)療應(yīng)用不具備足夠的精確度。如在現(xiàn)有技術(shù)中已知的，對于這種類型的系統(tǒng)，深度測量中的錯誤大，這是要禁止的。對于深度精確度，三角測量法具有射影法明顯的優(yōu)點(diǎn)。三角測量法，也稱為立體測量技術(shù)，由于執(zhí)行實時計算所需的昂貴的硬件而受到抵制。通過分區(qū)陣列傳感器的多個標(biāo)記三角測量法另外不存在標(biāo)記識別差的問題，這一點(diǎn)典型地通過人的介入而得到解決。以前的系統(tǒng)在存在偏離的IR源和導(dǎo)致不理想和不期望的標(biāo)記反射出的真實狀態(tài)下工作惡劣。以前的系統(tǒng)在存在多個相互靠近的物體的情況下也可能工作惡劣。
發(fā)明概述根據(jù)本發(fā)明，提供了一種用于測定多個物體中的每一個物體的空間位置和取向的系統(tǒng)。每一個物體具有至少三個標(biāo)記，它們具有預(yù)定的相對幾何關(guān)系。標(biāo)記適合于響應(yīng)激勵信號和／或從可激勵能量源撞擊到這種無源標(biāo)記上的反射能量而發(fā)射能量。設(shè)置一公共能量檢測器，用于測量由激勵標(biāo)記發(fā)射的能量和由無源標(biāo)記反射的能量。提供一公共處理器。該處理器具有存儲器。存儲器中存儲有每一個物體的標(biāo)記的預(yù)定的相對幾何關(guān)系。處理器將它所存儲的有關(guān)物體的標(biāo)記的預(yù)定的幾何關(guān)系與由能量檢測器測得的能量進(jìn)行比較，以識別發(fā)射／反射所測得的能量的物體。
通過這樣的安排，利用單個標(biāo)記放置方法，通過有力的積極標(biāo)記識別，在實時中可以使用非常簡單的操作順序，跟蹤物體。由此，可以同時跟蹤多個物體。
根據(jù)本發(fā)明的另一個特點(diǎn)，每一個物體必須具有標(biāo)記，它們具有已知并且固定的相對幾何關(guān)系，而且在所有標(biāo)記對中必須具有唯一的線段長度，其中，唯一一詞是指閾值差，該值建立在系統(tǒng)的精確度上(即，物體的標(biāo)記的幾何關(guān)系中的差是可以測量的)。如果在被跟蹤的所有物體中相似的段對具有唯一相對角度，則可以同時跟蹤多個物體，其中仍然是，唯一一詞是指建立在系統(tǒng)精確度基礎(chǔ)上的閾值差。根據(jù)應(yīng)用的需要，標(biāo)記幾何關(guān)系可以是共線或共面的。
另外，通過這樣的安排，將系統(tǒng)設(shè)置得適合于跟蹤具有已知相對幾何關(guān)系的3個或更多的標(biāo)記的物體的姿勢，這種幾何關(guān)系服從單個的定位規(guī)則，這不限于是非共面或非共線的。系統(tǒng)能夠使用成本低廉、低速的分區(qū)陣列傳感器，它可以在單個傳感器周期中跟蹤多個標(biāo)記，由此增加每一個標(biāo)記的出現(xiàn)取樣率。
根據(jù)本發(fā)明的另一個方面，系統(tǒng)使用傳感器的立體測量安排，由此為諸如外科應(yīng)用之類的高性能應(yīng)用提供適當(dāng)?shù)木_度。
另外，該系統(tǒng)能夠使用成本低廉的數(shù)字信號處理器，并使處理計算步驟簡單，它將自動和確定地識別物體的三維(3D)中的不連續(xù)的標(biāo)記，并在存在許多錯誤標(biāo)記和多個相互接近的物體的情況下工作。這種系統(tǒng)適合于使用單個傳感器周期圖像，而不是使用預(yù)言性方法，以閉式方案(closed formsolution)實時地測定一個或多個物體的姿勢，以連續(xù)跟蹤給予幾個傳感器周期圖像的物體的姿勢。
另外，這種系統(tǒng)適合于自動認(rèn)可和跟蹤各種在跟蹤之前已知的物體。
附圖概述參照下面聯(lián)系附圖的描述，本發(fā)明的其它特點(diǎn)將更加容易顯見，其中這些附圖是

圖1是根據(jù)本發(fā)明的測定一對剛體的空間位置和取向的系統(tǒng)的方框圖；圖2是示出適用于圖1的系統(tǒng)中的一對物體；
圖3是存儲在圖1的系統(tǒng)中使用的處理器的存儲器中的圖表的圖；圖4、5、6、6A-6B、7、7A-7C、8和8A-8B是流程圖，通過圖解說明圖1的系統(tǒng)操作的順序，其中圖7示出圖7A-7C之間的關(guān)系，而圖8示出圖8A和8B之間的關(guān)系；圖9A、9B到18說明在執(zhí)行圖1的系統(tǒng)的各個階段時，圖3的存儲器中的元件的詳細(xì)例子。
較佳實施例獨(dú)立的描述現(xiàn)在參照圖1，提供一系統(tǒng)10，用于測定一個或多個(這里提供了一對剛體11a、11b)剛體的空間位置和取向。這里，在圖2中更加詳細(xì)地示出和描述的剛體11a、11b是不同的手術(shù)器械。這里，剛體11a具有多個，這里是兩個無源回射標(biāo)記12a、12c，與一個有源發(fā)射標(biāo)記12b，3個標(biāo)記都粘貼在剛體11a上。這里，每一個能量回射標(biāo)記12a、12c包含一個球體，球體能夠粘貼到物體11a上，并覆蓋有現(xiàn)有技術(shù)中可以買到和已知的回射材料。標(biāo)記12a、12b、12c以預(yù)定的固定相對幾何關(guān)系粘貼到物體11a上。預(yù)定的固定相對幾何關(guān)系是由下面描述的簡單的定位確定的。
再參照圖1，系統(tǒng)10包含公共能量測量系統(tǒng)14，用于測量由粘貼到物體11a上的有源標(biāo)記12b發(fā)射的能量，以及由粘貼到物體11a上的無源標(biāo)記12a、12c反射的能量。公共測量器系統(tǒng)14分別包括一對隔開的，安裝在左邊和安裝在右邊的傳感器裝置14L和14R。每一個傳感器裝置14L、14R各自包含二維電荷耦合裝置(CCD)傳感器18L、18R(圖1)；聚焦透鏡22L、22R，如圖所示；和多個光能發(fā)射源24L、24R(這里，是紅外線能量發(fā)射二極管)如圖所示。
每一個傳感器裝配14R和14L具有其本身的u，v，zs坐標(biāo)，該坐標(biāo)分別按照它相關(guān)的定向紅外線能量源24L、24R排列。光發(fā)射源24L、24R均勻地分布在每個傳感器裝置14R和14L zs軸的圓周上。通過處理器部分26，將電能給予多個光發(fā)射源24L、24R。處理器部分26包括處理器28、主機(jī)30、顯示器32和控制器34。處理器28通過線36上的信號，將能量給予光發(fā)射源24L、24R。多個光發(fā)射源24L、24R進(jìn)行工作，以產(chǎn)生入射取向的紅外線能量束，其傳播取向?qū)?zhǔn)一般對應(yīng)于每一個與定向能量源24L、24R相關(guān)的傳感器裝置14L、14R的定向軸zs。由定向紅外線能量源產(chǎn)生的入射定向能量的尺寸、形狀和強(qiáng)度對應(yīng)于其相關(guān)的傳感器裝置14L、14R的測量體積的視場，并且足以在測量體積上提供入射定向能量束。
每一個傳感器裝置14L、14R都能夠在線39L、39R上產(chǎn)生輸出信號，這些信號表示傳感器裝配上聚焦的能量強(qiáng)度。在每一個傳感器周期中，其上聚焦的能量被收集(即集合)，然后轉(zhuǎn)移到處理器28。這里，將傳感器裝置14L、14R安裝到固定的坐標(biāo)系，并相互分開預(yù)定距離，這里是500mm。這里，傳感器裝置14L、14R分別具有一個視場，該視場足以觀察大致1立方米的公共測量體積，其中心沿zs軸離原點(diǎn)大約1．9m處，這是透鏡22L和22R的中間。
如上面提到的，每一個傳感器裝置14L和14R分別具有其本身有關(guān)的透鏡22L、22R，用于聚焦來自能量回射標(biāo)記12a和12c的反射能量以及來自能量發(fā)射標(biāo)記12b的發(fā)射能量，以在分別與傳感器裝配14R、14L相關(guān)的透鏡22L、22R上產(chǎn)生來自標(biāo)記12a、12b、12c的發(fā)射或反射能量的能量圖像。
將處理器28耦合到傳感器裝配14L和14R上，并測定每一個傳感器裝置14L和14R上聚焦的能量圖像的二維u、v位置。然后，利用每一個傳感器裝配14L、14R上相同的標(biāo)記12a、12b、12c的聚焦的能量圖像的u、v位置，產(chǎn)生左和右傳感器能量源位置表50L、50R(圖3和9A、9B)，并設(shè)置左-源和右-源計數(shù)器51L、51R，下面將對此進(jìn)行描述。
將處理器28耦合到主機(jī)30，以使物體11a、11b的空間位置顯示在顯示器32上，或進(jìn)一步由主機(jī)30處理。如上面提到的，處理器28被耦合到定向能量源24L、24R，以使處理部分26能夠在適當(dāng)?shù)臅r候激勵定向能量源24R和24L。并將處理器28耦合到控制器34，以便使處理器28能夠給控制器34發(fā)信號以在需要的時候通過線27激勵能量發(fā)射標(biāo)記12b。
下面將對于物體11a描述系統(tǒng)10的操作，應(yīng)該知道，對于其它剛體，諸如物體11b，操作的順序大致上是相同或相等的。由此，如圖所示，有源標(biāo)記12b通過電纜27(圖1)被饋送到控制器34。這里，如上面提到的，能量發(fā)射標(biāo)記12b包括一標(biāo)記12b的紅外線能量發(fā)射二極管，它在被賦予由控制器34通過電纜27饋送的電能時，發(fā)射紅外線光能。這種紅外線能量發(fā)射二極管是能夠買到的，并且是現(xiàn)有技術(shù)中已知的。
現(xiàn)在，更詳細(xì)地參照圖2，剛體11a和11b分別具有粘貼到它們上的標(biāo)記12a-12c和12d-12f。首先應(yīng)該知道，標(biāo)記12a和2c是回射的，以及標(biāo)記12b是有源的無關(guān)緊要。這種配置僅僅是范例，而下面描述的方法不依賴于標(biāo)記的類型。每一個物體11a、11b分別具有以預(yù)定(即，已知的)和固定相對幾何關(guān)系，粘貼到其上的標(biāo)記12a、12b、12c和12d、12e、12f。另外，標(biāo)記12a、12b和12c的相對幾何關(guān)系和物體11b的標(biāo)記12c、12d和12f必須能夠檢測出不同來。由此，如圖2所示，物體11a的標(biāo)記12a、12b和12c分別由線段SLab，SLbc和Slac分開。另外，如圖所示，線段SLab，SLbc和Slac交叉，形成角θab，θbc和θac。類似地，物體11b的標(biāo)記12d、12e和12f分別由線段SLde，SLef和Sldf分開。另外，如圖所示，線段SLde，SLef和Sldf交叉，形成角θde，θef和θdf。另外，線段長度SLab必須和線段長度Slac和線段長度SLbc不同，并且線段長度SLbc和線段Slac不同。在本較佳實施例中，變化Δ是5．0mm。由此，如果線段SLab的長度是SLab，線段SLbc的長度至少是SLab±Δ，而線段Slac的長度至少是SLab±Δ。即，線段SLab、SLbc和Slac的長度必須相差Δ。另外，有3個標(biāo)記的物體將具有3個線段。但是，通常，線段的數(shù)量等于N*(N-1)／2，其中N是標(biāo)記的總數(shù)。物體11a有一對線段SLab、SLbc，它們的長度和物體11b上的一對線段Slde、Slef相等；如果物體11a上的線段SLab、SLbc之間的相對角θab不同于物體11b上的線段Slde、Slef之間的相對角θde，仍然能夠跟蹤它們。根據(jù)應(yīng)用的需要，標(biāo)記幾何關(guān)系可以是在同一線上、不在同一線上，共面或非共面的。如果在物體11a、11b中被跟蹤的相同的一對線段具有唯一的相對角，可以同時跟蹤一對物體11a、11b，其中唯一這個詞仍然是指建立在相同10的精確度上的閾值差。即，將標(biāo)記12a-12c、12d-12f分別放置在物體11a、11b上，為每-個物體11a和11b提供能夠由處理器28承認(rèn)和跟蹤的唯一的簽名或指紋。
在討論系統(tǒng)10的操作前，首先應(yīng)該注意處理器28具有存儲器40(圖3)，該存儲器存儲3組表格，42-48；50L，50R和52；以及56-62。第一組表格(即剛體確定表格42-48)定義了每一個物體11a、11b的標(biāo)記12a-12d、12e-12f的標(biāo)記的預(yù)定的幾何關(guān)系；第二組表格(即傳感器產(chǎn)生的數(shù)據(jù)表，50L、50R和52)容納每一次傳感器14L、14R掃描時產(chǎn)生的信息，因此這些表50L、50R和52不和專門的物體有關(guān)系；還有，在剛體11a、11b被識別和跟蹤時由處理器產(chǎn)生的第三組表格(即，處理表56-62)。這些表格(和計數(shù)器51L、51R和53，下面將描述)在處理器28內(nèi)，并在處理器28的操作程序中由其使用。
剛體定義表42-48剛體定義表包含標(biāo)記位置表42；標(biāo)記線段表44；標(biāo)記線段點(diǎn)(·)產(chǎn)生表46和標(biāo)記線段組表48。這些剛體定義表，42-48是所有物體11a、11b的，并包含一已知的信息，該信息是關(guān)于分別粘貼到剛體11a、11b上的標(biāo)記12a-12c和12d-12f的幾何關(guān)系的，由此為每一個物體11a、11b提供唯一的，可由處理器28識別和跟蹤的簽名或指紋。這些剛體定義表42-48在處理器28的識別和此后的跟蹤操作一開始之前初始化。
標(biāo)記位置表42每一個剛體11a、11b具有它的標(biāo)記位置表42，如為圖11中的物體11a示出的。標(biāo)記位置表42包含與例如剛體11a相關(guān)的每一個標(biāo)記12a、12b和12c的3D位置(X’，Y’，Z’)。參照圖11，為物體11a示出標(biāo)記位置表42，應(yīng)該知道，表42對于物體11b具有類似的表。圖中示出標(biāo)記12a、12b和12c的3D位置。
線段長度表，44每一個剛體11a和11b具有與其相關(guān)的標(biāo)記線段長度表44(圖12)，該表44包含物體11a、11b的線段長度組。認(rèn)為線段是連接物體11a的標(biāo)記12a、12b、12c和物體11b的標(biāo)記12d、12e和12f中的一對標(biāo)記的直線。由此，如上面結(jié)合圖2所述，物體11a具有線段SLab、SLbc和SLac，而物體11b具有線段SLde、Slef和SLdf。物體的所有的線段就是每一對標(biāo)記的所有的組合。由此，對于物體有N*(N-1)／2個線段，其中N是粘貼到物體的標(biāo)記的數(shù)量。
參照圖12，為物體11a示出標(biāo)記線段長度表44。圖中示出線段長度SLab、SLbc和SLac。
標(biāo)記線段組表，48每一個剛體11a、11b具有與其相關(guān)的標(biāo)記線段組表48(圖13)，它包含標(biāo)記線段組。在表48中對于每一個標(biāo)記都有一個條目。每一個標(biāo)記條目將包含2個或更多連接到該標(biāo)記的線段。對于N個標(biāo)記的物體，將有N-1根線段連到每一個標(biāo)記。圖13示出剛體11a的線段組。每一個標(biāo)記12a、12b、12c具有其相關(guān)的2個線段(即，線段1和線段2，在圖13中)。由此，如在圖2和13中指出的，對于物體11a，將標(biāo)記12a貼到線段SLab和SLac；將標(biāo)記12b貼到線段SLab和SLbc。將標(biāo)記12c貼到線段SLac和SLbc。應(yīng)該知道，對于標(biāo)記11b有類似的表。
線段點(diǎn)(·)和表，46每一個剛體具有與其相關(guān)的線段點(diǎn)(·)積表46(圖14)，該表46包含每一個線段的組合之間產(chǎn)生的點(diǎn)(·)積的列表。當(dāng)將線段看作對X、Y、X系統(tǒng)10坐標(biāo)系進(jìn)行變換的矢量時，將點(diǎn)(·)和用于測定線段長度SL之間的角θ當(dāng)N是剛體中線段的數(shù)量時，將有N*(N-1)／2個線段對的組合。例子圖14示出物體11a的一組點(diǎn)(·)積。這里，對于線段長度SLab和SLbc之間的角θab的點(diǎn)(·)積顯示是3600。同樣的，線段長度SLab和SLac之間的角θa，c的點(diǎn)(·)積是0，而線段長度SLbc和SLac之間的角θb，c的點(diǎn)(·)積顯示是2500。應(yīng)該知道，對于物體11b存在類似的表。
傳感器產(chǎn)生數(shù)據(jù)表，50L，50R和52傳感器產(chǎn)生數(shù)據(jù)表50L、50R和52包含左邊和右邊傳感器能量源位置表50L、50R；和原3D標(biāo)記表52。
左邊和右邊傳感器能量源表50L、50R在存儲器40中，對于每一個分區(qū)陣列CCD傳感器18L、18R(圖1、圖9A、圖B)有一個傳感器能量源表50L和50R。對于在CCD傳感器18L、18R上測得的每一個能量點(diǎn)，有一個條目。左邊源和右邊源計數(shù)器51L、51R分別包含在左右傳感器18L、18R上分別測得的能量點(diǎn)的數(shù)量。每一個條目將具有一個U值和一個V值，它們對應(yīng)于能量點(diǎn)沿有關(guān)的傳感器18L、18R的U軸和V軸的質(zhì)心。在較佳實施例中，將有左右傳感器能量源表50L、50R(圖3)。這里，有四個能量源S1-S4，S5-S8，它們分別由每一個傳感器18L、18R測得，如圖9A和9B中指出的。注意，如圖9A和9B指出的，能量源S1-S4，S5-S8在傳感器18L、18R的u，v坐標(biāo)中。
原3D標(biāo)記表52在存儲器40中有單個原3D標(biāo)記表52(圖3、10)，它包含每一個測定的但是不合格的標(biāo)記位置(原標(biāo)記)。每一個條目有一個X、Y和Z(即X、Y、Z系統(tǒng)10的坐標(biāo)系)值，對應(yīng)于位置傳感器的坐標(biāo)系，其原點(diǎn)在圖像傳感器18L、18R之中間點(diǎn)。原標(biāo)記計數(shù)器53包含測得的原標(biāo)記的數(shù)量。參照圖10，給出對于4個原標(biāo)記的例子。在這個例子中，測得物體11a的標(biāo)記12a、12b和12c，和一個偏離為未知的標(biāo)記。此時，不知道這些標(biāo)記R1-R4分別關(guān)于物體11a和11b的標(biāo)記12a-12c和12d-12f的分配。將利用一系列的操作來測定這些標(biāo)記對物體11a或物體11b的對應(yīng)。
處理表56-62處理表是線段原標(biāo)記對照表56；合格線段表58；測量的標(biāo)記位置表60；和計算的剛體位置和取向(姿勢)表62。這些處理表56-62由處理器28，為每一個剛體11a、11b制定，并在識別(鑒別)和跟蹤剛體時由處理器28產(chǎn)生。
線段原標(biāo)記對照表，56每一個剛體具有一個與其相關(guān)的線段原標(biāo)記對照表56(圖3、15)，它包含所有對分開距離接近于剛體定義的線段長度的原標(biāo)記12a、12b和12c。接近詞定義為線段和測試中的線段之間的長度差，它小于某些預(yù)定值(即，能夠由系統(tǒng)10測量的距離)。在較佳實施例中，該值是1．5mm。下面的例子(圖15)說明了與物體11a預(yù)定的線段長度SLab、SLbc、SLac匹配的標(biāo)記對。原標(biāo)記對數(shù)據(jù)由處理器28根據(jù)一種方法測得，這種方法將結(jié)合圖6描述。但是，這樣說就足夠了，即，在本實施例中，由左右傳感器14L、14R(圖9A、圖9B)測得的兩組4個原能量數(shù)據(jù)S1-S8由處理器28轉(zhuǎn)換為4個原標(biāo)記R1-R4(在系統(tǒng)10的X、Y、Z坐標(biāo)系中)，并存儲在3D原標(biāo)記表52(圖10中)。由此，由于有4個原標(biāo)記R1-R4，故有6個線段長度(即，SL12、SL13、SL14、SL23、SL24和SL34)。這里，在本例子中，原標(biāo)記R1和R2分開線段長度SL12，該線段長度接近于線段SLab的長度。原標(biāo)記(R1，R4)，(R2，R3)分開接近于線段SLac的長度。原標(biāo)識(R2、R4)和(R1、R3)R分開接近于線段SLac長度的如圖15所示，這種數(shù)據(jù)存儲在線段原標(biāo)記對照表56中。
合格線段表，58對于每一個剛體11a、11b有一個合格線段表58(圖3、16)。該表58在線段檢驗階段產(chǎn)生，將結(jié)合圖7描述。但是，這么說就足夠了，即，對于剛體的每一根線段有一個條目。圖16的例子示出物體11a的線段SLab、SLbc和SLac都是合格的。
測量標(biāo)記位置表60每一個剛體11a、11b具有與其相關(guān)的測量的標(biāo)記位置表60(圖3、17)，它包含已經(jīng)鑒別、驗證、并繪制到物體實際標(biāo)記12a-12c，12d-12過的原標(biāo)記R1-R4的3D位置。圖17中給出的例子示出物體11a的標(biāo)記12a、12b和12c的測量位置，其中實際標(biāo)記12a對應(yīng)于原標(biāo)記R2，實際標(biāo)記12b對應(yīng)于原標(biāo)記R4而實際標(biāo)記12c對應(yīng)于原標(biāo)記R1。
計算的剛體位置和取向表，62每一個剛體具有與其相關(guān)的計算剛體位置和取向表62(圖3，18)，該表62包含剛體的變換。這是根據(jù)標(biāo)記位置表42(圖11)，從測量標(biāo)記位置表60(圖17)測定的姿勢。換句話說，該姿勢是將標(biāo)記位置表42轉(zhuǎn)移到系統(tǒng)10的相同的空間X、Y、Z坐標(biāo)系的變換。圖18所示的例子說明了物體11a的姿勢。
總的操作的順序可以從下面的流程圖4中所說明的工作中，同時和實時地測定物體或目標(biāo)的姿勢或取向。在步驟401中，激勵能量源24L和24R(圖1)和有源標(biāo)記12b。參照圖1，這些能量源通過透鏡系統(tǒng)22L、22R聚焦，并將圖像投射在DDC傳感器18L、28R上。該圖像從傳感器18L、18R掃描，并由處理器28分析在某一個閾值以上的所有強(qiáng)度。在步驟402，2傳感器能量源的位置存儲在左右傳感器能量源位置表50L、50R(圖3、9A、9B)中。位置以像素為單位。將傳感器的水平軸稱為U，并將豎直軸稱為V。在本較佳實施例中，使用左右傳感器18L、18R。分別將左能量源和右能量源計數(shù)器51L、51R設(shè)置為分別在左右傳感器18L、18R上測得的能量源的數(shù)量。在結(jié)合圖9A和9B描述的例子中，有四個分別由傳感器18L、18R測得的能量源S1-S4，S5-S8；由此，在該例子中，每一個計數(shù)器51L、51R中的計數(shù)是4。
在步驟403中，為以后的操作順序?qū)m當(dāng)?shù)谋砀?即，表52、56)和計數(shù)器51L、51R和53初始化。清除原3D標(biāo)記表52、原標(biāo)記計數(shù)器53和線段原標(biāo)記對照表56。在步驟404中，由處理器26分析存儲在左右傳感器能量源位置表50L、50R中的能量源，并產(chǎn)生原3D標(biāo)記表52。將原標(biāo)記計數(shù)器S3設(shè)置為測得的原標(biāo)記數(shù)量。此時，不知道這些標(biāo)記是什么。有一些可能是來自一個成數(shù)個被跟蹤的物體的標(biāo)記，另外的可能是反射，還有一些可能是由標(biāo)記測量方法引起的人工產(chǎn)物。由立體視圖產(chǎn)生3D位置的三角測量法是現(xiàn)有技術(shù)中熟知的技術(shù)，以下將在產(chǎn)生原3D標(biāo)記表52部分描述其一種方法。
在步驟405和406中，計算所有原3D標(biāo)記組合的對之間的距離，即線段長度SL12、SL13、SL14、SL23、SL24和SL34。對被跟蹤的每個物體11a、11b，將計算的長度與線段長度表44(圖12)進(jìn)行比較。為每一個物體11a、11b在線段原標(biāo)記對照表56(圖15)中設(shè)置匹配。這些步驟在圖6中聯(lián)系產(chǎn)生線段原標(biāo)記對照表56(圖15)詳細(xì)描述。
在步驟407中，通過比較物體11a、11b線段長度SL之間的相對角度，檢驗每一個物體11a、11b在線段原標(biāo)記對照表56(圖15)中的可能的線段推測。這些步驟將在下面聯(lián)系圖7的線段驗證進(jìn)行詳細(xì)描述。在步驟408中，通過使用物體的標(biāo)記線段設(shè)置表48(圖13)與物體的線段原標(biāo)記對照表56(圖15)的交點(diǎn)法，測定在上面給出的例子中的原3D標(biāo)記R1-R4和各個物體11a、11b的實際標(biāo)記12a-12c、12d-12f之間的對應(yīng)關(guān)系。將原3D標(biāo)記繪制到物體的測量標(biāo)記位置表60(圖17)中。這些步驟將在下面聯(lián)系圖8的標(biāo)記對應(yīng)提取詳細(xì)描述。
在步驟409中，通過包含在物體測量標(biāo)記位置表60(圖17)中的標(biāo)記測定物體的取向(姿勢)。從離散的標(biāo)記測定剛體的6個自由度的方法是現(xiàn)有技術(shù)中熟知的，這里將不作描述。最后，在步驟410中，將所有物體的姿勢存儲在表62(圖18)中，并且可以顯示。該姿勢還可以根據(jù)要求，進(jìn)行存儲、傳送到另一個計算機(jī)或進(jìn)一步進(jìn)行處理。通過使用下面的例子和詳細(xì)的描述，上述順序?qū)⒏尤菀罪@見。
原3D標(biāo)記表，52的產(chǎn)生分析存儲在左右傳感器能量源位置表50L、50R中的能量源S1-S4，S5-S8(圖9A和9B)，并測定原3D標(biāo)記R1-R4(圖10)。將原標(biāo)記計數(shù)器53設(shè)置為測得的原標(biāo)記的數(shù)量?，F(xiàn)在參照圖5，描述由兩個立體圖像產(chǎn)生3D位置的方法。下面的方法是現(xiàn)有技術(shù)中熟知的，并且可以使用其它方法。
在步驟501中，開始將原標(biāo)記計數(shù)器53設(shè)置為0。在步驟502、503、504和505中為每一個左傳感器能量源14L和每一個右傳感器能量源14R產(chǎn)生參數(shù)線議程式。一條線位于傳感器18L、18R中的一個傳感器上的一點(diǎn)(即在V=U=Zs，或傳感器坐標(biāo)系統(tǒng)原點(diǎn)處)和能量源S1-S4中的一個之間。由此，從傳感器18L到每一個測得的能量源S1-S4有4條線(即，左邊的線)。類似地，從傳感器18R到每一個測得的能量源S5-S8有4條線。在步驟506、507和512到515中，處理一條雙環(huán)路，它將使每一個左邊的線與每一個右邊的線成對。該環(huán)路反復(fù)的數(shù)量將等于左邊能量源52L*右邊能量源52R。在上述例子中，有4個左邊能量源S1-S4，和4個右邊能量源S5-S8(圖9A和9B)，這將對計算表52取16次反復(fù)(圖10)。
在步驟508和509中，測定左邊直線和右邊直線之間的最小距離。如果該距離小于預(yù)定的最小值，則認(rèn)為直線相交，并已經(jīng)發(fā)現(xiàn)可能的3D標(biāo)記R1-R4。由于此時它還未知是否是一個正確的標(biāo)識，故認(rèn)為該標(biāo)記是原標(biāo)記。在圖10所示的例子中有4個這樣的匹配。最小距離是系統(tǒng)精確度的函數(shù)，并保持盡可能小，以減小錯誤的標(biāo)記的數(shù)量。在步驟510和511中，在原3D標(biāo)記表52(圖10)中設(shè)置左右線之間的3D中點(diǎn)。原標(biāo)記計數(shù)器53增量。
在完成步驟501到515時，原標(biāo)記計數(shù)器53包含測得的原標(biāo)記的數(shù)量，并且完成原3D標(biāo)記表52(圖10)。從這一點(diǎn)起，可用3D標(biāo)記作出所有以后的決定，而且不再需要2D傳感器能量位置。對于這個例子，有4個測得的原3D標(biāo)記R1-R4。在這時不知道每一個標(biāo)記是什么。
產(chǎn)生所有剛體的線段原標(biāo)記對照表，56(圖15)在總的看法中，接著的程序?qū)?zhí)行如下。將表52(圖10)中的所有原3D標(biāo)記(即R1-R4)之間可能的線段與每一個物體11a和11b的圖12的表44中的所有線段長度SLab、SLbc、SLac和SLde、Slef、SLdf比較。如果發(fā)現(xiàn)有匹配，則將原3D標(biāo)記對加到該物體的線段原標(biāo)記對照表56(圖15)中。測試中的相同的原線段可能與幾個剛體匹配。同樣，一個剛體的線段可以具有幾個匹配的原測試線段。這些將在后面的操作，即線段驗證(圖7)中被濾去。
通過使用一個例子，和流程6，所有剛體的產(chǎn)生線段原標(biāo)記對照表，56，可以使上述內(nèi)容顯見。在步驟601、602和613到616中。控制兩個加深處理環(huán)路。最外面的兩個環(huán)路由計數(shù)器N和L標(biāo)指(但圖中未示)，但是包含在處理器26中，用于使所有原3D標(biāo)記R1-R4的組合配對。有N’*(N’-1)／2條線段(成對的)，其中N’等于例如原標(biāo)記R1-R4。這里，N’=4。每一次反復(fù)外面兩個環(huán)路時計算原3D標(biāo)記N和L之間的線段長度(步驟603)。該長度稱為試驗長度，并用于以下部分。
在步驟604、611和612中，由計數(shù)器J控制處理環(huán)路在所有被跟蹤的剛體中的順序，該計數(shù)器J圖中未示，但是包含在處理器28中。在步驟605、609和610中，由圖中未示但是包含在處理器28中的計數(shù)器K控制處理環(huán)路在所有物體11a、11b中的一個的所有線段的順序，其中這里以物體J統(tǒng)一表示。在步驟606、607和608中執(zhí)行線段的匹配。將物體J的線段K與試驗線段比較。如果差小于預(yù)定值，則認(rèn)為線段是匹配的線段。當(dāng)發(fā)生匹配時，原3D標(biāo)記對的指標(biāo)計數(shù)器N和L值被放置在物體J的線段K的線段原標(biāo)記對照表56(圖15)中下一對可用線段中。圖中未示但包含在處理器28中的計數(shù)器的“總的原對數(shù)”對于線段K增加。預(yù)先定義的值是系統(tǒng)精確度的函數(shù)，并保持實際地盡可能小，以減小線段匹配數(shù)量，但是大得足以避免丟棄正確的線段。
在參照圖15給出的例子中，對于物體11a，線段長度SLab具有單個匹配，為原3D標(biāo)記對R1和R2。線段長度SLbc具有3個匹配，為原3D標(biāo)記對R1和R4；R3和R4；以及R2和R3。線段長度SLac具有兩個匹配為原3D標(biāo)記對R2和R2；和R1和R3。很清楚，6個線段中的3個線段不是剛體11a的部分，必須通過線段驗證(圖7)消除。
線段驗證通過比較物體11a、11b的線段長度SL之間的相對角度θ來驗證每一個物體11a和11b的線段原標(biāo)記對照表56(圖15)中由原43D標(biāo)記對定義的所有可能的線段。通過使用下面的例子，檢查圖7、7A-7C所示的流程圖，這種方法將變得清楚。線段驗證操作具有五個深入控制環(huán)路。
在步驟701、726和727中，通過計數(shù)器L控制處理環(huán)路，它在所有被跟蹤物體中定序。在步驟701中，為被驗證的剛體L清除合格線段表，58(圖16)。在步驟702、724和725中由計數(shù)器J控制處理環(huán)路，它在所有物體L的線段長度SL中定序。如果線段J未合格，進(jìn)入控制環(huán)路，如在合格線段表，58(圖16)中所指出的。
如果合格線段表，58(圖16)中的線段J未合格(即設(shè)定為FALSE(步驟703))，則在步驟704、720和721中由計數(shù)器N控制處理環(huán)路，它在物體L的線段原標(biāo)記對照表56(圖15)中對于線段J中所有原3D標(biāo)記對定序。在步驟705中，為物體L的線段J的原3D標(biāo)記對N計算矢量X，該矢量轉(zhuǎn)換為系統(tǒng)10的坐標(biāo)軸原點(diǎn)。在步驟706、718和719中，由計數(shù)器K控制處理環(huán)路，它在物體L中線段J之后的所有線段長度中定序。
在步驟707、716和717中由計數(shù)器M控制處理環(huán)路，它在物體L對于線段K的線段原標(biāo)記對照表56(圖15)中的所有原3D標(biāo)記對定序。在步驟708到711中，為物體L的線段K的原3D標(biāo)記對M計算變化到始點(diǎn)的矢量Y。測定矢量X和矢量Y之間的點(diǎn)(·)積。將該點(diǎn)(·)積與存儲在物體L的線段點(diǎn)積表，46(圖14)中的線段對J和K的實際點(diǎn)(·)積比較。如果差值小于預(yù)先定義的值，則發(fā)現(xiàn)匹配。預(yù)先定義的值是系統(tǒng)10精確度的函數(shù)，并保持實際盡可能小，以減小錯誤線段匹配的數(shù)量，但是大得足以避免不必要地丟棄正確線段。本較佳實施例使用的值是200。
當(dāng)發(fā)現(xiàn)匹配時，認(rèn)為兩個線段都是有效的，并且認(rèn)為線段原標(biāo)記對照表，56(圖15)中的線段J和K的原3D標(biāo)記對N和M都是真的。在步驟712到715中，將原3D標(biāo)記對N和M移動到原J和K中第一個位置。將圖中未示的總的原對數(shù)計數(shù)器設(shè)置為1，并消除所有其它原3D標(biāo)記對。線段J和K設(shè)置為合格線段表，58(圖16)中的真值。當(dāng)完成控制環(huán)路J的每一次反復(fù)時，將在步驟722執(zhí)行一測試。如果在與所有以后的線段比較后線段J不合格，則認(rèn)為它是壞線段。在步驟723中，為線段J消除所有原3D標(biāo)記對，并將計數(shù)器總的原對數(shù)設(shè)置為0。對所有物體11a、11b(步驟725-728)重復(fù)這種處理。
現(xiàn)在將參照線段原標(biāo)記對照表，56(圖15)、線段點(diǎn)積表，46(圖14)，原3D標(biāo)記表，52(圖10)，和圖7的線段驗證流程圖，描述一個例子。下面的事件的順序可以在流程圖中進(jìn)行跟蹤。
L=物體11aJ=線段長度SLab線段長度SLab不合格。
N=SLab的對1(R1，R2)為對N(R1，R2)產(chǎn)生矢量K=線段長度SLbcM=線段長度SLbc的對1(R1，R4)為對M(R1，R4)產(chǎn)生矢量產(chǎn)生點(diǎn)積(·)。(R1，R2)·(R1，R4)=3599．9951117為SLab比較點(diǎn)積表，46(圖14)SLbc=3600發(fā)現(xiàn)匹配刪除線段長度SLbc的對2(R3，R4)和對3(R2，R3)。
設(shè)定線段長度SLbc為合格。
設(shè)定線段長度SLab為合格。
將M前進(jìn)到下一對。
這是最后一對線段長度SLbc。
將K前進(jìn)到線段長度SLac。
M=對1的線段長度SLac產(chǎn)生對M(2，4)的矢量產(chǎn)生點(diǎn)積(·)，(R1，R2)·(R2，R4)=0．01304為線段長度SLab，SLac=0比較點(diǎn)積表，46(圖14)發(fā)現(xiàn)匹配。
刪除線段長度SLac的對2(R1，R3)將線段長度SLac設(shè)置為合格將線段長度SLab設(shè)置為合格將M前進(jìn)到下一對這是最后一對線段長度SLab將K前進(jìn)到下一個線段這是最后一對線段。
將N前進(jìn)到下一對這是最后一對將J前進(jìn)到下一個線段長度SLbc該線段是合格的。
將J前進(jìn)到下一個線段。
這是最后一個線段。
將L前進(jìn)到下一個物體。
這是最后一個物體完成，見表(步驟158)標(biāo)記對應(yīng)提取在線段原標(biāo)記對照表，56(圖15)中指出的原3D標(biāo)記與物體實際標(biāo)記之間的對應(yīng)由交點(diǎn)法，使用物體的標(biāo)記線段組表48(圖13)測定。將原3D標(biāo)記R1-R4繪制到物體的測量標(biāo)記位置表，60(圖17)中。這些步驟使用流程8、標(biāo)記對應(yīng)提取，通過一個例子在下面描述。
標(biāo)記對應(yīng)提取操作具有三個深入控制環(huán)路。在步驟801、825以及826中，由計數(shù)器K控制處理環(huán)路，它在所有被跟蹤剛體中定序。在步驟802、823和824中由計數(shù)器N控制處理環(huán)路，它2在物體K的所有標(biāo)記中定序。使用寄存器M1和M2測定集合交點(diǎn)。在步驟803中初始地設(shè)置為空的。在步驟804、812和813中由計數(shù)器J控制處理環(huán)路，它在所有貼到物體K的標(biāo)記N的線段長度中定序。對于任何一給出的標(biāo)記，將有B-1個連接線段，其中B是物體的標(biāo)記的總數(shù)。
在步驟805中，為標(biāo)記線段集合表，48(圖13)中的物體K的標(biāo)記N，將L設(shè)定為由J指出的線段數(shù)量。在步驟807到811中，如果在線段原標(biāo)記對照表，56(圖15)中有合格線段長度SL，則測試標(biāo)記的集合交點(diǎn)。如果M1是空的，則將M1設(shè)置為這一對中的第一個標(biāo)記，而將M2設(shè)置為這對中的第二個標(biāo)記。如果M1不是空的，則進(jìn)行一個測試，以測定M1是等于這一對中的第一個或第二個標(biāo)記。如果M1不等于兩個標(biāo)記中的任何一個，則它不與集合相交，從而將設(shè)置為抵制。相等地，將測試M2是否等于這一對中的第一或第二個標(biāo)記。如果M2不等于兩個標(biāo)記中的任何一個標(biāo)記，則它不和集合相交，從而它將設(shè)置為剔除。這對所有連接到標(biāo)記N的線段重復(fù)執(zhí)行。
此時，M1和M2可以具有步驟814、815-817、818-820，以及821-822中測試的各種狀態(tài)。如果M1是正確的標(biāo)記，而M2是被剔除的，則由M1對照的原標(biāo)記對應(yīng)于標(biāo)記N。如果M2是正確標(biāo)記，而M1是剔除的，則由M2對照的原標(biāo)記對應(yīng)于標(biāo)記N。由M1或M2對照的原3D標(biāo)記位置可以拷貝到測量的標(biāo)記位置表，60(圖17)。如果M1和M2都剔除或空的，則對于標(biāo)記N沒有對應(yīng)的原標(biāo)記，而對于這個標(biāo)記，將測量標(biāo)記位置表，60(圖17)設(shè)置為丟失。
例子完成一個例子，將使上面的陳述清楚。參照線段原標(biāo)記對照表，56(圖15)，標(biāo)記線段集合表，48(圖13)，原3D標(biāo)記表，52(圖10)，測量標(biāo)記位置表，60以及圖8的標(biāo)記對應(yīng)提取流程圖。下面的事件的定序可以在流程圖中跟蹤。
K=物體11aN=標(biāo)記12a將M1和M2設(shè)置為空的。
將J設(shè)置為標(biāo)記12a的標(biāo)記線段集合表(圖13)中第一線段。
將L設(shè)置為線段長度SLab，由J對照。
在線段原標(biāo)記對照表，56(圖16)中有合格的SLab線段。原標(biāo)記對是R1，R2。
M1是空的，由此，設(shè)置M1=R1，M2=R2。
將J前進(jìn)到標(biāo)記12a的標(biāo)記線段集合表，48(圖13)中的第二個線段。
在線段原標(biāo)記對照表，56(圖15)中有合格的SLac線段。原標(biāo)記對是R2、R4。
M1不是空的。
M1不等于R2或R4，由此將M1設(shè)置為抵制的。
M2不等于R2。
將J前進(jìn)到標(biāo)記12a的標(biāo)記線段集合表，48(圖13)中的第三線段。
這是標(biāo)記N的最后一線段試驗M1和M2。
M1被抵制，M2等于R2，由此原3D標(biāo)記R2對應(yīng)于實際標(biāo)記N(12a)。
將3D拷貝到測量的標(biāo)記位置表，60(圖17)。
將N前進(jìn)到標(biāo)記12b。
對于標(biāo)記12b和12c重復(fù)上述程序。通過將原標(biāo)記R2映射到實際標(biāo)記12a，將原標(biāo)記R4映射到實際標(biāo)記12b，和將原標(biāo)記R1映射到實際標(biāo)記12c重復(fù)該處理。
測定物體取向(姿勢)
現(xiàn)在，我們具有所有測定剛體的位置所需要的信息。這在現(xiàn)有技術(shù)中是熟知的，并且這里不再描述。將姿勢存儲在計算的剛體位置和取向表，62(圖18)中。其它實施例將在所附權(quán)利要求的主旨和范圍內(nèi)。
權(quán)利要求
1．一種用于測定多個物體中每一個物體的空間位置和取向的系統(tǒng)，其特征在于包含至少三個以預(yù)定的相對幾何關(guān)系，附加到所述每一個物體的標(biāo)記，所述標(biāo)記適合于響應(yīng)激勵信號發(fā)射能量和／或反射從一可激勵的能量源射到這一無源標(biāo)記上的能量；能量檢測器，用于測量由所述有源標(biāo)記發(fā)射的能量和由所述無源標(biāo)記反射的能量；具有存儲器的處理器，所述存儲器中存儲所述每一個物體的所述標(biāo)記的預(yù)定的相對幾何關(guān)系；并且其中，所述處理器將所存儲的每一個物體的標(biāo)記的預(yù)定的幾何關(guān)系與由所述能量檢測器測得的能量進(jìn)行比較，以識別發(fā)射／或反射所檢測到的能量的物體。
全文摘要
本發(fā)明提供了一種用于測定多個物體中的每一個物體的空間位置和取向的系統(tǒng)。每一個物體具有至少3個標(biāo)記,它們具有預(yù)定的相對幾何關(guān)系。標(biāo)記適合于響應(yīng)激勵信號和/或反射由可激勵的能量源照射到這種無源標(biāo)記上的能量。設(shè)置一公共能量檢測器,用于測量由激勵標(biāo)記發(fā)射的能量以及由無源標(biāo)記反射的能量。設(shè)置一公共處理器。處理器具有存儲器。存儲器中存儲了每一個物體的標(biāo)記的預(yù)定的相對幾何關(guān)系。處理器比較所存儲的每一個物體的標(biāo)記的預(yù)定的幾何關(guān)系與由能量檢測器測得的能量,以識別發(fā)射或反射測得的能量的物體。通過這樣的安排,可以使用非常簡單的操作程序,利用簡單的標(biāo)記放置方法,通過有力的確定的標(biāo)記識別,實時跟蹤物體。由此可以同時跟蹤多個物體。
文檔編號G01B21/00GK1283272SQ98811823
公開日2001年2月7日 申請日期1998年12月2日 優(yōu)先權(quán)日1997年12月5日
發(fā)明者S·E·萊斯 申請人:北方數(shù)字股份有限公司