專利名稱：運動跟蹤系統(tǒng)的制作方法
技術領域：
本發(fā)明涉及運動跟蹤領域。
可以使用各種測量方式進行運動跟蹤，其中包括慣性和聲學測量方式，用于確定物體的位置和方向。
慣性運動跟蹤基于測量圍繞一組基本上正交的軸線的線加速度和角速度。在一種方法中，多個和被跟蹤物體相連的旋轉的回轉儀響應被跟蹤物體的旋轉而產(chǎn)生和其轉軸的轉速成正比的力。這些力被測量并用于估算物體的角速度?？梢允褂没谖⒓庸さ恼駝釉凸獠▽У难b置代替回轉儀。
加速計產(chǎn)生和由線加速度產(chǎn)生的力成正比的信號。在慣性跟蹤系統(tǒng)中，角速度和加速度信號被綜合用于確定線速度、線位移和轉動的總的角度。
由于由回轉裝置產(chǎn)生的信號含有噪聲，所以綜合處理導致噪聲成分的積累，這一般被稱為“漂移”。小型的和成本低的回轉裝置一般具有較大的誤差。對于一個靜止的物體，漂移速率可以高達每秒幾度，對于轉動的物體，每轉90度可發(fā)生幾度的漂移。方向估算中的誤差也影響位置估算，因為估算的物體的方向在進行綜合之前被用于把加速度的度量轉換為固定的參考框架。在這種轉換中的不精確性可以引起導致水平加速度測量偏差的重力。
用于校正漂移的一種方法是附加的檢測器，例如傾角計和指南針，用于間歇地或連續(xù)地校正綜合的慣性測量的漂移。例如，1997年7月8日公開的Eric M.Foxlin的美國專利5645077中披露了一種這樣的方法。該專利在此列為參考。
用于運動跟蹤的另一種方法使用聲波測量在物體上的一個或幾個點和在環(huán)境中的固定的參考點之間的距離。在一種被稱為“outside-in”的結構中，一組在物體的固定點上的聲發(fā)射器發(fā)射被在環(huán)境中的固定參考點的一組麥克風接收的脈沖。從發(fā)射器到麥克風經(jīng)過的時間和發(fā)射器與麥克風之間的距離的估算值(即距離)成正比。從發(fā)射器到接收麥克風的范圍估算值被用于通過三角測量法確定發(fā)射器的位置。在物體上的多個發(fā)射器的位置被組合，用于估計物體的方向。
其它的測量方法，例如對物體上的光源的光學跟蹤的方法也可以用于跟蹤物體的運動。
一般地說，在一個方面中，本發(fā)明是一種用于跟蹤物體的運動的方法，其包括以下步驟獲得和物體的運動相關的兩種類型的測量，其中一種類型的測量包括聲學測量，根據(jù)兩種測量中的一種測量例如根據(jù)慣性測量更新物體的方向或位置的估算，并且根據(jù)兩種測量中的另一種測量例如根據(jù)聲學測距更新所述估算。
在另一個方面，一般地說，本發(fā)明是一種用于跟蹤物體運動的方法，包括以下步驟選擇一組參考裝置中的一個裝置，向所選擇的參考裝置發(fā)送控制信號，例如借助于發(fā)送無線控制信號，接收來自參考裝置的距離測量信號，接收關于對選擇的參考裝置的距離的測量，并使用所接收的距離測量更新物體的位置估算和方向估算。所述的方法還可以包括根據(jù)距離測量信號的行程的時間確定距離測量。
本發(fā)明的優(yōu)點包括提供一種6個自由度的跟蹤能力，其可以在基本上不受限制的空間內應用，在所述空間內安裝有可以擴展的超聲波信標星座。慣性測量方法提供運動的平滑的和響應的檢測，而超聲波測量方法提供實時的誤差校正，例如由系統(tǒng)的慣性跟蹤元件的漂移引起的誤差的校正?？梢允褂猛ǔ＞哂邢喈敶蟮钠频男〉暮土畠r的慣性檢測器，此時仍然可以提供具有有限的漂移的整個系統(tǒng)。體積小和重量輕的慣性檢測器非常適用于虛擬的或增強真實性的顯示系統(tǒng)的頭戴式跟蹤裝置。通過使用超聲波測量校正漂移，可以不需要可能對外部因素例如磁場的改變等敏感的漂移校正測量。超聲波信標的星座在每個信標獨立地工作因而不需要在信標當中布線時可以容易地被擴展。在任何時刻，跟蹤裝置只依賴于使用少量的超聲波信標，借以允許跟蹤裝置工作的空間具有不規(guī)則的區(qū)域，例如在大樓中的多個房間。
本發(fā)明的另一個優(yōu)點在于，通過使用“inside-out”結構，使得在聲學距離測量中沒有在聲波發(fā)出之后由于物體的運動而引起的等待時間。
本發(fā)明的另一個優(yōu)點在于，通過使用慣性測量，即使在例如由于信標的閉塞而不能進行聲學測量時，也能繼續(xù)進行跟蹤。此時一旦慣性跟蹤的漂移被校正，可以再繼續(xù)進行聲學測量。
在另一個優(yōu)點中，本發(fā)明提供非全方位(Line-of-sight)冗余度，借以可以阻斷在發(fā)送器和檢測器之間的一個或幾個路徑，同時仍然能夠進行物體的跟蹤。
本發(fā)明的其它特點和優(yōu)點從下面的說明和權利要求中可以清楚地看出。


圖1表示跟蹤裝置和用于跟蹤所述裝置的聲學信標的星座；圖2表示跟蹤裝置處理器的元件；圖3表示組合的慣性和聲學跟蹤方法；圖4表示慣性測量單元(IMU)；圖5表示超聲距離測量單元(URM)和超聲信標；圖6表示用于跟蹤裝置處理器中和慣性測量單元以及超聲測量單元連接的輸入/輸出接口；圖7a表示導航和物體參考框架；圖7b表示相互跟蹤裝置；圖8表示慣性跟蹤器的信號流程圖；圖9表示超聲距離測量子系統(tǒng)的信號流圖；圖10是包括慣性跟蹤器和卡爾曼預報器以及更新器元件的跟蹤裝置的信號流圖；圖11是卡爾曼預報器的信號流圖；圖12是卡爾曼更新器的信號流圖；圖13是跟蹤程序的信號流圖；圖14a表示關于第一被跟蹤物體的第二物體的跟蹤；圖14b表示多個裝置的相互跟蹤；圖15表示頭戴式顯示系統(tǒng)；圖16表示用于電視的攝像機跟蹤系統(tǒng)；以及圖17表示在汽車上的物體的跟蹤。
參看圖1，一直進行其位置和方向估算的跟蹤裝置100在一個大的房間內自由運動。例如，跟蹤裝置100可以被固定在操作者的頭上的頭戴式顯示器(HUD)上，當操作者運動并且改變其頭的方向時，跟蹤裝置100通過房間而運動，并且改變其方向。跟蹤裝置100包括處理器130，其和用于提供關于線加速度和轉動速度的慣性測量的慣性測量單元(IMU)140相連。當跟蹤裝置100在房間內運動時，處理器130使用慣性測量確定跟蹤裝置100的運動。
處理器130還和用于接收從超聲信標陣列120即信標“星座”發(fā)出的超聲信號的3個超聲距離測量單元(URM)110耦聯(lián)。超聲信標陣列120包括獨立的超聲信標122，它們被設置在環(huán)境中的固定位置，例如被設置在一個大的房間的天花板上，例如呈間隔為2英尺的規(guī)則的網(wǎng)格圖案。處理器130使用來自特定的超聲信標122的信號和已知的所述信標的三維位置，估算到這些信標的距離，借以檢測跟蹤裝置100的運動。每個超聲信標122響應從跟蹤裝置100發(fā)出的紅外指令信號112發(fā)送超聲脈沖114。具體地說，在跟蹤裝置100上的每個URM110對所有的超聲信標122發(fā)送紅外(IR)信號。這些IR信號包括地址信息，使得只有一個信標或者只有少量的信標識別為其發(fā)送的IR信號，并且響應所述信號。響應IR信號，被尋址的信標立即發(fā)送超聲脈沖，該脈沖然后被一個或幾個URM110接收。處理器130得知被尋址的信標立即響應IR指令時，便通過測量從發(fā)出IR指令到檢測到超聲脈沖時間延遲確定行程的時間。超聲脈沖的行程時間被用于估算信標的距離，然后利用該距離更新跟蹤裝置100的位置和方向。
基于慣性測量和超聲信號的兩種測量方法具有局限性，單獨依靠哪一種測量方法取得的結果都不會比利用兩種方法的組合所得的結果精確。跟蹤裝置100組合兩種測量方式的測量結果。跟蹤裝置100在得到兩種測量方式的測量結果時，或者經(jīng)過一個時間延遲，便調整其位置和方向(即6個自由度“6-DOF”)的估算，以便反映兩種測量方式的測量結果。為此，處理器130控制一個擴展的卡爾曼濾波器(EKF)，其被用于組合所述的兩種測量，并保持不斷的估算跟蹤裝置100的位置和方向，并且一直保持在那些測量結算中不確定性的估算。
參看圖2，處理器130包括中央處理單元(CPU)200，例如Intel80486微處理器，程序存儲器220，例如只讀存儲器(ROM)，和工作存儲器230，例如動態(tài)隨機存取存儲器(RAM)。CPU200還和用于提供與IMU140、URM110的連接的輸入輸出接口210耦聯(lián)。輸入輸出接口210包括用于提供和IMU140、URM110的數(shù)字接口的數(shù)字邏輯。
IMU140提供一個對慣性測量結果編碼而獲得的串行數(shù)據(jù)流201。輸入輸出接口210把這個串行數(shù)據(jù)轉換成為一種并行的形式212，以便傳遞給CPU200。每個URM110接收串行信號211，該信號用于驅動IR發(fā)光二極管510向超聲信標122(圖1)發(fā)射IR控制信號。輸入輸出接口210接收來自CPU的用于識別一個或幾個超聲信標的地址信息，并向每個URM110提供串行信號，然后把所述串行信號加于發(fā)射的IR信號上(例如通過幅值調制)。同一個串行信號被提供給所有的URM110，它們同時發(fā)送相同的IR信號。每個URM110依次向輸入輸出接口210提供超聲脈沖到達的邏輯信號202。輸入輸出接口210包括用于確定來自信標的超聲脈沖的行程時間并用于確定到信標的距離估算的計時器。這些距離估算被提供給CPU200。
一種跟蹤算法被存儲在程序存儲器220中并由CPU200執(zhí)行，用于把從輸入輸出接口210獲得的測量值轉換為位置和方向估算值。CPU200還和固定數(shù)據(jù)存儲器240相連，所述存儲器例如包括超聲信標的位置的預定圖形和URM110的麥克風的位置的信息。處理器130還包括通信接口260，用于使CPU200和其它裝置相連，例如顯示裝置280，其根據(jù)跟蹤裝置100的位置和方向修改其顯示。
通過參看圖3可以理解系統(tǒng)的操作，圖3是圖1所示的房間的二維平面圖(從上看)。一列空白的圓圈和箭頭310a-e表示在一個時間段序列的每個時間段跟蹤裝置100的實際位置和方向。根據(jù)以前的測量結果，并根據(jù)在第一時間段的慣性測量結果，實心的圓圈和箭頭312a表示在第一時間段由跟蹤裝置100估算的跟蹤裝置的位置和方向。在下一個時間段，跟蹤裝置100運動到位置310b，根據(jù)新的慣性測量結果，跟蹤裝置100將其位置估算更新到312b。對于下一個時間段重復上述處理，得到實際的位置310c和估算的位置312c。
在到達位置310b之后，跟蹤裝置100發(fā)送一個IR指令，如虛線320所示，其具有一個超聲傳感器122的地址。在收到IR指令之后(基本上沒有延遲)，超聲傳感器122發(fā)送超聲脈沖，如波324所示。波324經(jīng)過一定時間到達跟蹤裝置100，此時跟蹤裝置的實際位置是330。當波326到達跟蹤裝置時，根據(jù)到達的時間，跟蹤裝置100估算其在位置332。
在下一個時間段，跟蹤裝置100首先根據(jù)慣性測量估算其位置312d。使用關于超聲傳感器122的位置和位置332間隔的距離信息以及測量的超聲波的行程時間，跟蹤裝置100計算一個精確的位置估算312d’。在實際位置310e和估算位置312e使用慣性測量重復所述的處理。
一般地說，在每一個時間段可以使用慣性測量和超聲測量，雖然可以較少地使用超聲測量。在每一個時間段，位置和方向(姿勢)被更新。通過使用相互隔開一定距離的多個麥克風，超聲脈沖可以提供關于位置和方向的信息。
參看圖4，慣性測量單元(IMU)140包括3個角速度檢測器(例如微加工的振動轉動檢測器或小的旋轉回轉儀)420a-c和3個線加速度檢測器410a-c。這些檢測器沿著在跟蹤裝置100的參考框架中保持固定的3個正交的軸線排列。每個加速度檢測器提供基本上和沿著相應的軸線的加速度成正比的信號，每個角速度檢測器提供基本上和圍繞相應的軸線旋轉的速度成正比的信號。
當慣性測量單元140的方向改變時，信號例如加速度信號則相應于在房間的固定(導航)參考框架內方向的改變。慣性測量單元140還包括信號接口430，其接收來自6個加速度計和加速度檢測器的每個的信號411，并發(fā)送一個串行數(shù)據(jù)流413，其多路傳輸加速度和加速度信號的數(shù)字表示。如同下面要討論的，加速度和角速度信號是不精確的，其具有附加的偏移和比例誤差。這些比例和偏移誤差可能和裝置的運動有關。
參見圖5，每個超聲測量單元110包括紅外(IR)發(fā)光二極管(LED)510，其由IR信號發(fā)生器512驅動。信號發(fā)生器512接收來自輸入輸出接口210(圖2)的串行信號211，并驅動IR LED 510向一個或幾個超聲信標122發(fā)送所述信號。要測量其距離的超聲信標的地址被在串行信號211中編碼。每個超聲信標122包括IR檢測器540，如果在超聲距離測量單元110和所述超聲信標之間具有足夠短的無障礙路徑，則所述IR檢測器接收所述IR檢測信號，然后所述信號被IR信號譯碼器542譯碼。這個譯碼的信號包括由超聲距離測量單元發(fā)送的地址信息?？刂齐娐?60接收譯碼的IR信號，并且確定是否那個超聲信標是真正要被尋址的，如果是，便通知脈沖發(fā)生器552對超聲傳感器550提供一個信號，使其產(chǎn)生超聲脈沖。所述脈沖通過空氣到達超聲距離測量單元110，其中麥克風520接收超聲脈沖并向脈沖檢測器522傳遞相應的電信號，脈沖檢測器522產(chǎn)生一個表示脈沖的到達的邏輯信號。所述脈沖檢測信號被傳遞到輸入輸出接口210(圖2)。如同下面所要討論的，利用行程時間測量距離是不十分精確的。其中的誤差源包括脈沖檢測中的定時誤差，例如由于空氣溫度或氣流以及從超聲信標發(fā)出的超聲波沿不同的傳播方向的不一致性而引起的聲音傳播速度的改變。
輸入/輸出接口210包括用于實施圖6所示的邏輯功能的電路(即可編程的邏輯陣列)。IMU數(shù)據(jù)緩沖器630接收來自IMU140的串行編碼的加速度和角速度數(shù)據(jù)413，并作為輸出向CPU200提供6個加速度角速度測量值631。輸入/輸出接口210還包括信標地址緩沖器610。CPU200(圖2)提供要測量其距離的超聲信標的地址。信標地址緩沖器610存儲所述地址，并以串行的形式向每個URM110提供所述地址。在由每個URM110發(fā)送(以及由超聲信標122接收)地址的同時，3個計數(shù)器620a-c被復位，并開始從0以固定的時鐘速率(例如2MHz)增加。當每個URM110檢測到來自信標的脈沖時，相應的脈沖檢測信號被傳遞給相應的計數(shù)器，其便停止計數(shù)。然后，CPU200利用所述計數(shù)值作為超聲脈沖從超聲信標到達每個URM110的行程時間的測量值。參見圖7a-b，跟蹤裝置100(圖1)確定其在房間的導航參考框架中的位置，如標號為N(北)、E(東)、和D(下)的軸線710所示。位置r(n)730是一個矢量，其具有分別沿著N、E和D方向離開軸線710的位移分量(rN(n),rE(n),rD(n))T。跟蹤裝置100還確定其姿勢(方向)。
參見圖7b，物體的方向按照為對準物體的方向所需的滾動角、俯仰角和偏轉(歐拉)角表示為θ=(Ψ,θ,Φ)T，物體的方向由坐標軸720表示，導航方向由坐標軸710表示。這3個歐拉角被表示為3×3的方向余弦矩陣Cbn(θ)，其在參考物體框架內變換坐標的矢量，其中主要按照順序應用圍繞z,y和x軸的偏轉、俯仰和滾動運動。方向余弦矩陣可以定義如下 Cbn的上標和下標表示該矩陣取在“b”(物體)參考框架內的矢量，并提供在“n”(導航)參考框架內的矢量。
參看圖8，包括旋轉檢測器420a-c和加速度檢測器410a-c的慣性檢測器800向慣性跟蹤器810提供慣性測量信號。慣性跟蹤器810執(zhí)行圖8中所示的信號流的離散的時間近似。首先，回轉儀補償820修正(矢量)角速度信號ω，以便把測量偏差考慮進去。在本例中，只校正附加的偏差δω。其它的偏差例如乘積誤差(例如不正確的比例系數(shù))，以及由于安裝不精確而引起的誤差也可以被校正。加速度計補償830類似地校正加速度信號a(b)中的附加偏差δa(b)。如同下面要充分討論的，使用超聲波測量計算包括偏差項δω和δa(b)。
方向積分840根據(jù)進行過偏差校正的旋轉信號更新方向估算。在本例中，使用方向的方向余弦表示進行方向積分。連續(xù)的微分方程Cbn(t)=Cbn(t)S(ω(t))的離散的時間表示被用于以固定的速率更新方向余弦矩陣，一般每秒在100和200次之間。通過改變對于離散的時間系統(tǒng)的表示(例如Ck=Cbn(kΔt))，按下式實現(xiàn)方向余弦矩陣的離散時間更新Ck=Ck-1(I+sinδθδθS(δθ‾)+1-cosδθδθ2S(δθ‾)2)]]>其中δθ‾=‾ωk-1+‾ωk2Δt,δθ=||δθ‾||]]>以及S(δθ‾)=0-δθzδθyδθz0-δθx-δθyδθx0]]>是δθ的斜邊對稱矩陣。注意S(δΘ)滿足下式S(δθ)2=δθ2I-δθδθT.
為了確保Ck確實是方向余弦矩陣，在每次迭代之后其行被正交化(orthonormalized)，以便消除可能被輸入到每項中的數(shù)值或近似誤差。
根據(jù)被跟蹤的方向余弦矩陣Ck，坐標變換850接收在物體參考框架內進行過偏差校正的加速度信號，并按照下式輸出在導航參考框架內的加速度信號二重積分850按照下式計算速度和位置v‾k(n)=v‾k-1(n)+a‾k-1(n)+a‾k(n)2Δt,]]>且r‾k(n)=r‾k-1(n)+v‾k-1(n)Δt+2a‾k-1(n)+a‾k(n)6Δt2•]]>
歐拉角計算部分870接收方向余弦矩陣，并輸出相應的歐拉角。慣性跟蹤器810的輸出是(θ,r(n))T。慣性跟蹤器的狀態(tài)包括一個15維的矢量，其由5組三維值組成如同下面詳細說明的，慣性跟蹤器810接收從超聲距離測量導出的誤差更新信號δx，用于校正方向、速度和位置值，并用于更新回轉儀和加速度計偏差校正元件的參數(shù)。
參見圖9，信標順序器910接收來自慣性跟蹤器810的位置估算r(n)。信標順序器910使用超聲信標122(圖1所示)的位置(和地址)的信標圖形915，確定在每一個時間段哪一個信標被觸發(fā)，以便產(chǎn)生超聲距離測量信號。例如，信標順序器910確定和當前位置最接近的信標，并在每一個時間段在這些信標當中循環(huán)。當位置估算改變時，一般地說，最接近的一組信標也改變。在信標順序器910依次觸發(fā)每個信標之后，相應的超聲脈沖到達并且被跟蹤裝置檢測到。每個脈沖產(chǎn)生對于每個用于檢測該脈沖的麥克風產(chǎn)生一個距離測量值。在本實施例中，每個脈沖產(chǎn)生一組3個距離測量值，每個測量值來自3個URM110中的一個麥克風。
繼續(xù)參看圖9，距離測量920相應于接收超聲距離估算的處理。用于距離測量的相關的參數(shù)是被尋址的信標的位置b(n)，用于檢測所述脈沖的麥克風的位置m(b)，距離估算本身dr，以及脈沖被檢測的時間tr，其被用于校正測量中的等待時間。注意，如果位置估算沒有誤差，因而距離估算完全精確，則距離估算應當滿足dr=‖b(n)-(r(n)(tr)+Cbn(tr)m(b))‖使用來自這個等式的偏差校正慣性跟蹤器810的參數(shù)和輸出。
跟蹤裝置100使用互補卡爾曼濾波器當輸入距離測量值時，通過不斷地更新被跟蹤的數(shù)量改進跟蹤的位置和方向的估算。參看圖10，所述的方法涉及兩個相關的元件。當慣性跟蹤器810更新其輸出x時，卡爾曼預測器1010保持x中的誤差的計算的協(xié)方差矩陣P。例如，在慣性跟蹤器810中沒有任何漂移補償時，協(xié)方差矩陣P將相應于一直在增加的誤差。
在這種方法中使用的第二個元件是卡爾曼更新器1020，其接收來自距離測量920的信息，并使用這個測量信息確定估算積累的誤差δx，其被送回慣性跟蹤器810，用于更新x。此外，在每個超聲測量之后，卡爾曼更新器1020計算在更新之后在x中的誤差的新估算的協(xié)方差矩陣，其被回送到卡爾曼預測器1010。每個超聲測量部分地校正慣性跟蹤器810的輸出。連續(xù)的超聲更新序列確保誤差為最小。
慣性跟蹤器810是一種對其輸入進行非線性處理的處理器，因此，由高斯噪聲驅動的用于純線性濾波器的卡爾曼濾波器的結構是不合適的。通過使用一般被稱為“擴展的卡爾曼濾波器”(EKF)，便可以使用一種線性化的動態(tài)系統(tǒng)模型，其表征在慣性跟蹤器810的輸出x中的誤差的傳播。由EKF模擬的誤差是δx=(φ,δω(b),δr(n),δv(n),δa(b))T具有相應于慣性跟蹤器的矢量輸出的分量。注意，在物體坐標系統(tǒng)中模擬誤差項δa(b)，而不在導航坐標系統(tǒng)中模擬，并且其它元件直接相應于慣性跟蹤器810的輸出中的誤差。線性化的誤差傳播模型的參數(shù)包括狀態(tài)轉移矩陣，和用于驅動所述誤差模型的驅動噪聲的協(xié)方差矩陣。狀態(tài)轉移矩陣和驅動噪聲協(xié)方差矩陣依賴于慣性跟蹤器的輸出。在沒有任何測量值的情況下，誤差處理的平均值保持為0。不過，誤差的協(xié)方差增加。誤差傳播的線性化模型是δxk=F(xk-1)δxk-1+wk-1·其中的項Fk=F(xk-1)是從在慣性跟蹤器810中使用的更新方程的擾動分析中導出的，并且相應于下面的誤差傳播方程φk=φk-1-Cnbδωk-1,δωk=δωk-1,δrk=δrk-1+Δtδvk-1-12Δt2(Cbnδak-1(b)-S(φk-1)(ak-1(n)+(0,0-g)T))]]>δyk=δvk-1+Δtδa(b)k-1-ΔtS(φk-1)(a(n)k-1+(0,0,-g)T),andδa(b)k=δa(b)k-1其中處理噪聲wk的協(xié)方差被假定為是對角線的。這些協(xié)方差矩陣的項是從對慣性跟蹤器810提供的慣性測量值中的已知的誤差源中得到的，包括附加的偏移誤差、比例誤差、檢測器和物體軸線的對準誤差和來自檢測器本身的信號噪聲。各個變量依賴于慣性跟蹤器的輸出，如下式表示Qk=diag(σφx2,σφy2,σφz2,σω2,σω2,σω2,σrx2,σry2,σrz2,σvx2,σvy2,σvz2,σa2,σa2,σa2)]]>其中的各個變量項按照下式被參數(shù)化σφx=GyroscaleωxΔt+GyroAlign(ωy+ωx)Δt+GyroNoiseΔr]]>σφy=GyroScaleωyΔt+GyroAlign(ωx+ωz)Δt+GyroNoiseΔt]]>σφz=GyroScaleωzΔt+GyroAlign(ωx+ωy)Δt+GyroNoiseΔt]]>σω= GyroBiasChangeRate Δtσrx=σry=σrz=0]]>σvx=AccelScaleaxΔt+AccelAlign(ay+az)Δt+AccelNoiseΔt]]>σvy=AccelScaleayΔt+AccelAlign(ax+ay)Δt+AccelNoiseΔt]]>σvz=AccelScaleazΔt+AccelAlign(ax+ay)Δt+AccelNoiseΔt]]>σ2a=AccelBiasChangeRate Δt其中GyroScale,AccelScale,GyroAlign,AccelAlign相應于在用于儀器誤差補償?shù)男氏禂?shù)中的不確定性的程度。一般地說，可以使用非對角線處理噪聲協(xié)方差。
參見圖11，卡爾曼預測器1010具有兩級，誤差初始化級1110首先按照上述計算Fk和Qk。然后，協(xié)方差傳播級1120利用迭代方法更新誤差協(xié)方差，其中在每個時間段使用卡爾曼協(xié)方差傳播方程Pk= Fk-1Pk-1FTk-1+Qk當卡爾曼預測器1010接收到作為超聲距離測量結果而產(chǎn)生的更新的協(xié)方差P(+)時，則更新的協(xié)方差代替當前誤差協(xié)方差P。
參見圖12，卡爾曼更新器1020接收來自慣性跟蹤器810的距離測量920的輸出以及位置和方向的估算值，以及來自卡爾曼預測器1010的位置和方向估算的誤差的協(xié)方差，并計算誤差估算和由應用所述誤差估算而得到的更新的協(xié)方差。卡爾曼更新器1020的第一級是測量余項計算1210。期望的距離和測量的距離之間的差利用下式計算δdr=dr-‖b(n)-(r(n)(tr)+Cnb(tr)m(b))‖.
注意，一般地說，在最初檢測到距離測量之后的某個時刻使用距離測量。為了考慮這個時延，使用在超聲脈沖到達時的跟蹤裝置的位置和方向而不使用在測量時的位置和方向。使用當前的位置、方向以及線速度和角速度估算，向回推斷測量時間，從而確定r(n)(tr)和Cbn(tr)。
為了應用卡爾曼更新方程，使用下面的線性化觀測方程模擬這個余項δdr=H(x,b,dr,m)δx+v.
觀測矩陣Hk=H(xk.b,dr,m)是位置和方向中的誤差對距離測量誤差的線性影響。附加噪聲v具有偏差R(xk,b,dr,m)。
Hk具有以下形式HK=(bDmg-bgmD+rgmN-rDmgdr,bNmD-bDmN+rDmN-rNmDdy,bgmN-bNmg+rNmg-rgmNDr,]]>0,0,0,rN+mN-bNdr,rg+mE-bEdr,rD+mD-bDdr,0,0,0,0,0,0,)]]>導出偏差R(xk,b,dr,m)用于模擬和超聲距離測量相關的各種現(xiàn)象。例如，當距離增加時，脈沖檢測更加困難，這部分地由于脈沖擴散，因而使用增加的距離模擬相關的距離測量誤差。偏差R(xkb,dr,m)具有以下形式R=σ2u+σ2t并被參數(shù)化為σ2u=NoiseFloor+NoiseScale dr以及σ2t=(kΔt-tr)Hk(ωx,ωy,ωz,0,0,0,vx,vy,vz,0,0,0,0,0,0))THk的前兩項可以交替地被設置為0，以便進行加速度測量傾斜校準(如果需要更精確)。如果第三項被設為0，則用一個較長的時間間隔進行偏轉漂移校正，不過精度較高。
卡爾曼更新器1020包括測量接收/拒絕級1230。接收/拒絕級1230接收測量余項δx，和測量余項的計算的偏差R。如果測量余項的大小大于計算的測量余項的標準偏差的一個預定的倍數(shù)，則該測量因被懷疑而被拒絕，例如，這種測量結果可以由于超聲脈沖檢測器被提前或推遲觸發(fā)而得到。否則，測量余項便被進一步處理，以便計算狀態(tài)誤差估算δx。使用卡爾曼濾波更新方程，卡爾曼增益計算1240按下式計算卡爾曼增益
K=PkHTk(HkPkHTk+R)-1然后，誤差估算1250計算誤差估算為δx-K δd，并且協(xié)方差更新器1260計算更新的誤差協(xié)方差如下P(+)=(I-KH)Pk,然后使用δx的分量更新慣性跟蹤器810。計算出的項δω和δa(b)分別被送到回轉儀偏差校正單元820和加速度計偏差校正單元830(圖8)，在那里被分別附加到當前存儲的偏差參數(shù)上．計算的項δv(n)和δ(n)被分別送到重積分單元860(圖8)，在那里它們被分別附加到v(n)和r(n)的當前估算上。最后，按照下式更新方向余弦矩陣Ck-(I-s(φ))Ck,并被重新進行正交化。
再次參看圖1，超聲信標陣列120包括被排列成規(guī)則圖案的各個超聲信標122。例如，這些信標可以被排列成間距大約為2英尺的方格，最好具有3mm或更小的精度。對這些信標設置有限數(shù)量的地址，在本例中由于硬件的限制只利用8個不同的地址。因此，當跟蹤裝置向一個地址發(fā)出IR指令時，一般地說，將有多個超聲信標接收信號并響應所述信號。只有具有任何特定地址的最近的一個信標用于距離測量。不過，因為有多個信標可以響應每個IR指令，脈沖檢測電路可能被過早地觸發(fā)，例如通過來自在以前的重復中的被觸發(fā)的信標的脈沖，但是該信標足夠遠，使得其發(fā)出的脈沖直到下一次重復才到達。為了避免這個預觸發(fā)問題，脈沖檢測器522(圖5)只在預計所需的脈沖要到達的時間窗口被選通。這便避免了由來自其它信標的脈沖或由前面的脈沖的長時間的不斷的反射而形成的信號造成的誤觸發(fā)。
在本說明中，用于跟蹤和卡爾曼更新的程序，初始位置和方向估算被假定為是已知的。但是不一定是這種情況，跟蹤裝置100可以使用自動獲取算法。有限數(shù)量的超聲信標的地址作為初始獲取算法的基礎。首先，跟蹤裝置觸發(fā)具有每個可允許的地址的信標，并測量到每個地址的最接近的一個信標的距離。然后，根據(jù)距離測量確定4個最接近的信標的地址。跟蹤裝置包括一個信標圖形，所述圖形包括所有信標的位置和地址。這些信標被這樣排列，使得4個最接近的信標的地址把可能的位置局限于房間的一個小的部分。如果根據(jù)最接近的信標不能確定，則使用到信標的實際的距離進行三角測量處理來進行確定。最初的方向基于對于每個麥克風的相對距離測量。
整個跟蹤處理可以被歸納為圖13所示的流程圖。首先，使用上述的方法獲得初始位置和方向(步1310)。然后程序進入每次處理一個時間段的循環(huán)。在進入等待下一個時間段之后(步1320)接收初始測量(1330)，并且被跟蹤的變量x和誤差協(xié)方差P通過使用慣性測量被更新(1340)。如果得到尚未處理的超聲距離測量(1350)，則使用距離測量計算誤差更新δx，并更新誤差協(xié)方差P(+)(1360)。然后使用誤差更新和新的誤差協(xié)方差更新慣性跟蹤器和卡爾曼預測器(1370)。然后程序確定是否在該時間段必須進行距離測量(1380)。因為對于每個脈沖進行3個距離測量，但是每個時間段只進行一次距離測量，所以可能有在下一個時間段將要進行的積累的距離測量。因此，對于若干個將來的時間段不必進行新的距離測量。考慮到下一個超聲脈沖的預期的行程時間(其一般大于一個時間段)，程序確定是否在該時間段應當發(fā)送IR指令(1380)，如果是，則選擇下一個信標地址(1390)，如果不是，則向那個信標發(fā)送IR指令(1395)。然后程序在步1320再次開始循環(huán)，等待下一個時間間隔的開始。
可以使用若干個不同的方法。在上述的實施例中，每個時間段只使用一次距離測量。此外，如果處理器130具有足夠的計算能力，則在每一個時間段可以使用所有可利用的距離測量。這另一種方法通過從步1370向回循環(huán)到步1350直到進行所有的距離測量來實現(xiàn)。另外，除去對每一個距離測量的數(shù)量依次進行卡爾曼更新之外，可以在每個時間段中都可以使用類似的更新方程更新向量觀測和相關的觀測噪聲。此外，除去直到下一個時間段進行不同的距離測量處理之外，當?shù)玫骄嚯x測量的結果時，都可以將其考慮進去，而不和慣性跟蹤器的更新同步。
上述的程序可以和其它測量方式組合。例如，可以使用傾角計對擴展的卡爾曼濾波器提供允許校正方向漂移的測量。此外，除去使用3個或多個允許進行所有的3個旋轉度的校正的麥克風之外，可以使用兩個麥克風和所述傾角計組合進行距離測量。用這種方式，可以根據(jù)傾角計進行一些漂移校正，但是不需要用指南針進行漂移校正，因為其對磁場的變化敏感。也可以使用3個以上的麥克風提供較大的冗余度，并且允許更多的旋轉自由度。
作為在環(huán)境中的固定位置安裝信標和在跟蹤裝置上設置麥克風的通常被稱為“inside-out”結構的一種替代方案，有一種相反的被稱為“outside-in”的結構。此時跟蹤裝置提供超聲脈沖，而具有坐標的麥克風陣列檢測跟蹤裝置的位置。注意在脈沖到達麥克風的時刻，一般地說，跟蹤裝置已經(jīng)移動到一個新的位置。這種重復的測量必須利用和上述的距離測量中使用的重復補償相似的方式進行補償。
信標122不必被設置在一個平面陣列中，它們可以被設置在墻壁上和天花板上，或者被設置在環(huán)境中的其它支撐物上。例如，信標可以被設置在燈具固定物上。信標的數(shù)量可以被選擇，以便和用戶的要求匹配，并且可以根據(jù)各種準則選擇信標的位置，例如可利用的合適的固定點和幾何方面的考慮，并且信標的圖案可以被設置使得和選擇的信標的位置和數(shù)量相匹配。在星座中信標的數(shù)量可以由用戶增加或減少，只要信標圖形保持更新即可。
從跟蹤裝置到信標的指令信號可以使用IR發(fā)送之外的其它方式發(fā)送。例如可以使用RF，可見光或聲波。跟蹤裝置也可以通過線路和信標相連。
利用“inside-outside-in”結構可以跟蹤兩個或多個物體。參見圖14a，跟蹤裝置100和上述一樣跟蹤其位置。第二個跟蹤裝置1400包括按照已知的相互位置關系排列的3個可尋址的超聲信標1410。通過觸發(fā)信標1410發(fā)送由跟蹤裝置100上的URM110接收的超聲脈沖，跟蹤裝置可以確定第二個跟蹤裝置的相對位置和方向。一個進一步的擴展提供相對位置和方向估算的增加的精度，其涉及使用被固定在跟蹤裝置1400上的第二慣性測量裝置和向跟蹤裝置100發(fā)送慣性測量。如果只在第二物體上設置單個的信標，則可以使用超聲距離測量檢測相對位置，而不需跟蹤第二個裝置的相對方向。
參見圖14b，可以使用由多個跟蹤裝置構成的“相互跟蹤網(wǎng)絡”。這些跟蹤裝置跟蹤相對于環(huán)境中的其它裝置的各自的位置，在所述環(huán)境中包括固定的信標和其它運動的被跟蹤的物體。這可以利用和跟蹤裝置相連的附加的通信系統(tǒng)來實現(xiàn)，例如RF局域網(wǎng)。
在上述的實施例中，信標陣列的“圖形”被假定為是精確的。因為距離測量包括冗余的信息，在信標布局中的誤差可以用迭代方法消除并被更新，借以改善精度。具體地說，信標的布局誤差可被包括在擴展的卡爾曼濾波器的狀態(tài)中，并且來自每個信標的距離測量將隨時用于布局誤差的估算。也可以使用一種單獨的初始自動的“成形”方式，其中通過在房間內的一個或幾個位置的距離測量和三角測量計算，可以確定信標的位置。這些被自動確定的位置可被用作已知的位置，或者用作初始的估算，其然后通過使用卡爾曼濾波器被進一步更新。在這種類型的方法中，信標可被無規(guī)則地設置在房間內，而不需要被精確地定位。
上述的跟蹤方法有若干種應用。第一種應用涉及使跟蹤裝置和頭戴的顯示器相連。參見圖15，頭戴顯示器1510使用戶能夠直接觀看物理對象1520，例如工件。顯示器1510使用工件1520在房間參考框架中的已知位置，把信息疊加在用戶觀看工件的視野中。例如，在對大型裝置施加一個線束時，疊加的信息可以包括關于線束的正確放置的信息。也可以使用類似的頭戴顯示器在虛擬的現(xiàn)實系統(tǒng)中提供被用戶觀看的完整的圖像，而不把圖像疊加在由用戶觀看的實際圖像上。
另一種實際應用涉及跟蹤電視拍攝中的攝像機的位置。參看圖16，電視攝制中的一般技術是在空白(一般是單色的)背景的前方拍攝物體1620，然后以電子方式疊加作為背景的另一個圖像(表示為1630)。這種技術的困難在于，當攝像機1610運動時，背景圖像應當改變，以便反映攝像機的運動。通過在照相機1610上設置跟蹤裝置100，可以跟蹤攝像機的位置和方向，通過用于接收攝像機的改變的位置和方向的圖像處理器自動地修正背景圖像。這種方法使得能夠構成大的被存儲在圖像處理器中的“虛擬場景”，借以可以使用多個并且不斷改變的攝像機的角度。
另一種應用涉及檢測汽車中的物體的運動，例如，在汽車碰撞試驗中。參看圖17，在碰撞的汽車1710內的假人1720可以使用跟蹤裝置100進行跟蹤。此外，第二個物體例如防火墻上的一點可以使用上述的inside-outside-in方法利用信標1730進行跟蹤。這使得能夠在汽車的參考框架內跟蹤假人并且相對于假人跟蹤汽車內的一點。
其它的應用包括自動導航，庫存物品、資產(chǎn)或人員的跟蹤，用于破壞控制的虛擬的或擴大真實性的船只，攝像機的跟蹤，娛樂(例如音樂主題和游戲節(jié)目)的跟蹤，用于運動捕獲的整個物體的跟蹤，以及武器的跟蹤。
另一個實施例也可以使用其它方法進行慣性跟蹤。例如，不使用方向余弦矩陣進行方向積分，可以同樣地使用歐拉角或四元法進行方向積分。注意線性化誤差傳播系統(tǒng)矩陣和驅動噪聲協(xié)方差在一定程度上取決于使用的具體的跟蹤算法。此外，可以改變卡爾曼濾波器的狀態(tài)，例如以便包括其它的項。這方面的一個例子不僅跟蹤加速度計附加偏移，如上面的實施例中所述，而且跟蹤加速度計信號、誤對準和聲音速度的乘積偏差。
也可以使用其它的距離測量方法，包括聲音相位，RF或光的行程時間，RF或光的相位，以及機械電纜延伸。
也可以使用熔合(fusing)慣性和聲音測量的其它方法代替卡爾曼濾波方法。例如，神經(jīng)網(wǎng)絡，基于規(guī)則的推理，或模糊邏輯系統(tǒng)，或者使用優(yōu)化方法使這些測量方法相結合。
在上面的說明中，只使用了8個不同的超聲信標地址。此外，每個信標可以被單獨地編址，或者可以使用大量的共用地址。如果信標能夠被單獨地尋址，可以進行初始獲取，例如，通過使信標也響應“組地址”，或者以這種方式在獲取階段響應對各個信標尋址的指令序列，使得跟蹤裝置通過首先找到在范圍內的一個信標對其初始位置“zero in(歸零)”，然后根據(jù)跟蹤裝置已知的信標圖形檢索越來越接近的信標。當跟蹤區(qū)域由幾個不同的房間構成時也可以使用這種方法。首先，確定跟蹤裝置所在的房間，然后確定其在房間中的位置。
應當理解，上面的說明只是用于說明本發(fā)明，而不限制本發(fā)明的范圍，本發(fā)明的范圍應由所附的權利要求限定。其它的方面、優(yōu)點和改型都落在下面的權利要求的范圍內。
權利要求
1．一種用于跟蹤物體的運動的方法，其包括以下步驟獲得和物體的運動相關的兩種類型的測量，其中一種類型的測量包括聲學測量；根據(jù)兩種測量中的一種測量更新物體的方向或位置的估算；以及根據(jù)兩種測量中的另一種測量更新所述估算。
2．如權利要求1所述的方法，其中一種類型的測量包括聲學距離測量。
3．如權利要求1所述的方法，其中另一種類型的測量包括慣性測量。
4．如權利要求1所述的方法，其中所述估算是方向估算。
5．一種用于跟蹤物體運動的裝置，包括兩個檢測器系統(tǒng)，其被構成分別用于獲得和物體的運動相關的兩種類型的測量，其中一種類型的測量包括聲學測量；以及和所述兩個檢測器系統(tǒng)相連的處理器，其被構成用于根據(jù)兩種測量中的一種測量更新物體的方向或位置的估算，并根據(jù)兩種測量中的另一種測量更新所述估算。
6．一種跟蹤裝置，包括檢測器系統(tǒng)，其中包括慣性檢測器；和一組和慣性檢測器剛性連接的一個或幾個聲音檢測器；以及處理器，其被編程用于執(zhí)行以下功能接收來自慣性檢測器的慣性測量結果；使用接收的慣性測量結果更新位置估算和方向估算；選擇多個聲學參考裝置中的一個；接收關于在檢測器系統(tǒng)和選擇的聲學參考裝置之間的距離的聲學距離測量結果；使用接收的距離測量結果更新位置估算和方向估算。
7．如權利要求6所述的跟蹤裝置，其中檢測器系統(tǒng)包括用于發(fā)送控制信號編碼和選擇的聲學參考裝置的識別符的發(fā)送器，并且每個聲學檢測器包括用于接收來自聲學參考裝置的聲音信號的麥克風。
8．如權利要求6所述的跟蹤裝置，其中所述一組一個或幾個聲學檢測器包括兩個或多個聲學檢測器。
9．如權利要求6所述的跟蹤裝置，其中使用接收的慣性測量結果更新位置估算和方向估算包括更新位置和方向估算中的不確定性；以及使用接收的慣性測量結果更新位置估算和方向估算包括確定距離測量中的不確定性，并使用距離測量中的不確定性更新位置和方向估算中的不確定性。
10．一種用于跟蹤物體運動的方法，包括以下步驟選擇多個參考裝置中的一個裝置；向所選擇的參考裝置發(fā)送控制信號；接收來自參考裝置的距離測量信號；接收關于對選擇的參考裝置的距離的測量；以及使用所接收的距離測量更新物體的位置估算和方向估算。
11．如權利要求10所述的方法，包括根據(jù)距離測量信號的行程的時間確定距離測量。
12．如權利要求10所述的方法，其中發(fā)送控制信號包括發(fā)送無線控制信號。
13．一種被存儲在加速計可讀介質中的軟件，包括用于使計算機執(zhí)行以下功能的指令選擇多個參考裝置中的一個裝置；向所選擇的參考裝置發(fā)送控制信號；接收來自參考裝置的距離測量信號；接收關于對選擇的參考裝置的距離的測量；以及使用所接收的距離測量更新物體的位置估算和方向估算。
14．一種跟蹤系統(tǒng)，包括聲學參考系統(tǒng)，其中包括多個聲學參考裝置；以及跟蹤裝置，包括檢測器系統(tǒng)，其包括慣性檢測器和一組與慣性檢測器剛性連接的一個或幾個聲音檢測器；以及處理器，其被編程用于執(zhí)行以下功能接收來自慣性檢測器的慣性測量結果，使用接收的慣性測量結果更新位置估算和方向估算，選擇多個聲學參考裝置中的一個，接收關于在檢測器系統(tǒng)和選擇的聲學參考裝置之間的距離的聲學距離測量結果，使用接收的距離測量結果更新位置估算和方向估算。
15．如權利要求14所述的系統(tǒng)，其中檢測器系統(tǒng)包括用于發(fā)送控制信號編碼和選擇的聲學參考裝置的識別符的發(fā)送器，并且每個聲學檢測器包括用于接收來自聲學參考裝置的聲音信號的麥克風，并且其中每個聲學參考裝置包括用于接收來自檢測器系統(tǒng)的控制信號的接收器和用于發(fā)送聲音信號的聲學傳感器。
全文摘要
本發(fā)明涉及物體運動的跟蹤,它通過獲得和物體的運動相關的兩個類型的測量來實現(xiàn)的,一種類型的測量包括聲音測量。根據(jù)兩種類型的測量中的一種測量,例如根據(jù)由慣性測量(140)單元獲得的慣性測量更新物體位置或方向的估算。然后根據(jù)兩種測量中的另一種例如根據(jù)由聲學距離測量單元(110)獲得的聲學距離測量更新所述的估算。本發(fā)明的特征還在于對于選擇的參考裝置確定距離測量,所述的參考裝置被固定在所述物體的環(huán)境中,例如被固定在超聲信標陣列(120)中的超聲信標(122)。
文檔編號G01S5/22GK1308505SQ99807510
公開日2001年8月15日 申請日期1999年4月8日 優(yōu)先權日1998年4月17日
發(fā)明者E·M·?？怂沽?申請人:麻省理工學院