TW497967B - Motion tracking system - Google Patents

Description

五、發明說明（1) 本發明係古 移動、έ 、有關於移動 移動追蹤可 量測模式，^ / Μ 慣性移動读疋—物體 度之量測為基 所附著之追縱物之旋轉而 比例之力4。這些力量被 度。製作精細之振動元件 加速度測量儀產生與 信號。在—‘償’!生追縱系統 在一起以決定速度、位移 由於旋轉儀裝置所產 過私造成雜訊成份的累積 你型及低價的旋轉儀通常 之物體所得到之飄移率可 9 0度時，其飄移率亦有數 將加速度之量測結果進行 方向估測上的誤差也會影 不準碟可以造成重力加速 值。 式’包括 向。 之線性加 ，多個旋 同時產生 來估測物 可以取代 造成之力 及加速度 度。 常雜亂， 被稱為「 的誤差。 數度，當 體方向之 為基礎之 測。在這 向上有一 追蹤。 同的量測模 之位置與方 一組垂直軸 —個方法中 產生轉速， 量測且被用 及光導裝置 線性加速度 中’角速度 及總旋轉角 生之信號非 ’這種現象 會產生較大 以到達每秒 度。由於物 以參考架構 響位置的估 度在水平方 慣性及聲音 速度及與速 轉儀會依其 與此轉速成 體之角速 旋轉儀。 量成比例之 信號被合併 信號合併的 飄移」。迷 對固定不動 物體每轉動 估測被用來 轉換，所公 種轉換中的 加逮度偏 正飄移現象方法之一是，使用額外的感測器， 角計及指南針，以不斷地修正慣性量測結果之飄&現σ傾 舉例來說，美國專利第5 645 〇77號揭露了此種方法。 。 另一個方法是，使用聲波來量測一物體上一個或兩個

第4頁 497967 五、發明說明（2) 點與量測環境中 r outs i de- i n j 器發射脈衝，再 從發射器至麥克 (測距）成比例。 三角測量法測量 置被組合起來以 其他的量測 可以 下步 包括 性量 據該 測加 括以 信號 自該 測距 之測 新。 結果 用來追縱一 本發明之一 驟。首先， 聲音量測法 一參考點 ，是使用 由位於參 風之時間 從發射器 發射器之 估測物體 模式，如 物體。 特徵為， 提供兩種 。依據該 測法，將一該物體 兩種量測法 以更新。 本發明之另 下步驟。首 至該被選擇 參考裝置之 量測結果與 距量測結果 該方法更包 之 中之另一 一特徵， 先，從複 之參考裝 測距量測 至該被選 將一該物 括依據測 間之距離。有一種方式被稱為 一組在物體上一定點之聲音發射 考點上之一組麥克風接收。聲波 與發射為、與麥克風間之估測距離 至相對麥克風間之測距被用來以 位置。在物體上多個發射器之位 之方向。 利用物體上光源之光學追蹤，也 提供一追蹤物體之方法，包括以 該物體運動之量測法，其中一種 兩種量測法中之一量測法，如慣 位置或方向之估測加以更新。依 里測法，如聲音量測法，將該估 係提供一種追蹤物體之方法，包 數參考裝置中擇一。發送一控制 置’如無線控制信號。接收一來 信號。接受一測距量測結果，該 定之參考裝置有關。使用該接受 體之位置估測或方向估測加以更 距信號之飛行時間決定測距量測 本發明之優— ”、、匕括提供一自由度為6之追蹤能力，可

$ 5頁 497967 五 發明說明（3) 以在一無限制之空間中發揮 超音波信標群。慣性量測法提此空間可以存有一擴展 力，同時超音波量測則提供同^ 了平滑及敏感的物體感測 由系統之慣性追蹤飄移所產生^ ^决差修正功能，如那些 位之慣性感測器通常會造成較、誤差。雖然逑你型及低價 會使系統產生無限制之飄移；f飄移，但其使用仍然不 器非常適用於虛擬實境顯示系、重量輕之慣性感測 超音波量測修正飄移現象，1 碩頂式追蹤。藉由使用 便不再需要。•音波信標4: 移修正量測 單獨使用且不需在信標間做連線。母信標 僅需小量之超音波信標即可，因衣在任何蚪候， 空間可以是不規則的，如一楝：：：追縱裝置所操作之 本發明之另-優點為，：m中，數個方間。 法，使聲音測距量測結果中不會有延遲現lnide-〇ut」方 現象係由於在聲波發射後物體繼續移動所造S生，此延遲 本發明又一優點為，即使聲音量測因信 進行，依然可以持續進行追蹤。慣性追蹤中^I ，無法 等到聲音量測恢復後便可繼續進行。 T 、t正則 本發明又一優點為，提供了多條的監視路徑，# 即使一條或更多條發射器與感測器間路徑被错此’（ 進行繼續進行追蹤。 ’亦可以 為讓本發明之上述目的、特徵及優點能更 下文特舉較佳實施例，並配合所附圖式，作七·、、、員易懂， 下。 外、、、田說明如 第6頁 497967 五、發明說明（4) 圖式簡單說明 圖1顯示一追蹤裝置及一用以追蹤該裝置之聲波信標 群。 圖2顯示一追蹤裝置處理器之組件。 圖3顯示一慣性-聲波組合式之量測方法。 圖4顯示一慣性量測單元。 圖5顯示一超音波測距量測單元及一超音波信標。 圖6顯示一追蹤裝置處理器與慣性量測單元及超音波 量測器元間之輸出/入界面。 圖7A顯示物體及物體移動之參考架構。 圖7B顯示相互追蹤裝置。 圖8係一慣性追蹤器之信號流。 圖9係一超音波測距量測子系統之信號流。 圖1 0係一具有慣性追蹤器及Ka 1 man估測器之追蹤器， 以及更新器元件。 圖11係一 Kalman估測器之信號流。 圖12係一Kalman更新器之信號流。 圖1 3係一追蹤流程圖。 圖1 4 A顯示一相對於一第一被追蹤物之第二物體之追 蹤動作。 圖1 4B顯示多個裝置之相互追蹤動作。 圖1 5顯示一頭頂式顯示系統。 圖1 6顯示一電視攝影追蹤系統。 圖17顯示對一車輛内之物體的追縱動作。

497967 五 發明說明（5) 貫施例 明參閱圖1，一追蹤裝置 …、，該裝置持續地對本身之 1在—大房間中移 :?:追縱裝置uo可以被固定在置及做估測。舉例來 房t1中移動並改變方向。追==移動及轉動在此 =夏測單元U0之處理器13〇，二 包括—耦接至-慣

Ml加速及轉速之慣性量測。早兀獒供了相關 於房間中移動時’使用慣性 二130在追蹤裝置丨00 處理器U0亦輕接至一由二果」寻出其移動動作。 成之陣列，㈣列係用以接收二來個^音距單元110所組 之聲音信號’即一信標群。超 波信標陣列120 量剛環境中定點之獨立超4;車:⑽包括位於 天花板上，且以h尺見方之格狀^ 22_被女裝在房間的 某—特定超音波信標122之j M °處理1113^使用來自 離並感測追縱裝置100之移；：每以些信標之距 自追縱裝置H。之紅外線丄；二個超：波信標122依來 rr^ σ! 1沐相7 1口唬11 2之指示送出一超音波 二Π是，每「個在追縱裝置1〇〇上之超音波測 兀▲ / 紅外線彳5唬至所有的超音波信標1 2 2。這些 、，工外線信號包括位址資料，使得只有一個或少數個信標可 以識別其相對之紅外線信號，並產生回應。—載有位址之 信標在回應一紅外線信號時，會立刻送出一超音波脈衝， 此脈衝被一個或多個超音波量測單元nQ所接收。由於處 理态1 3 0可識別此回應紅外線信號之載有位址之信標，處 497967 五、發明說明（β) 理器1 3 0便藉由量測紅外 :,遲時間得出信號之來回Ά出至接收到超音波脈衝 ^標之距m，然後再將此距離二:此時間值被用來估測 位置及方向。 以更新追縱裝置〗〇〇之 慣性量測與超音波俨旦 兩者較單獨使用1量測法：:!均有—些限制。混合使用 100結合了兩種量測模式並同斤付·^結果精確。追蹤裝置 其運動及方向之估測。為了 據兩種量測結果來調整 一擴充之Kalman濾波器之運、此目的，處理器130控制 量測結果並保持對追蹤裝置】〇 ’此濾波器被用來結合兩種 時也保持對該些估測之不確定产之位置^及方向之估測，同 請參閱圖2，處理器13〇包括一 ^ 2 Intel 80486微處理器）、_ ^央處理單元2〇 〇(如 體）及一工作儲存哭如 式儲存器220 (如唯讀記憶 保存杰230 (如一動態隨機存取纪愔舻、由止 貝οΊ40及超音波量測單元110間之界面。輸出) ίίΓ曰:邏輯電路，提供與慣性量測單元140及 超音波Κ測％元丨丨〇間之數位界面。 怳性$測單元1 40提供一載有慣性量測結果之序列資 料流201。輸出/入界面210將此序列資料流轉換為一平行 格式21 2以傳送至中央處理單元2 〇 〇。每一個超音波量測單 元11 0接叉一序列信號21 1以驅動一紅外線發光二極體5丄〇 而送出一紅外線控制信號至超音波信標i 22。輸出/入界面 2 1 0接受來自中央處理單元2 0 0之位址資料以識別一個或多 497967 五 發明說明（7) _____ 個起音波^標，並提供序列信號至每一 110而將此序列信號以紅外線傳輸送出—個超音波量測單元 亦提供給所有超音波測距單元i i 0，之2问樣的序列信號 線信號。每一個超音波測距單 ，迗出同樣的紅外 2心至輸出/入界面21。，顯示超音以m輯信號 二21。具有計時器’可以得出超音波脈：丄達：：出/入界 間，因而可以估測至信標之距離。 。‘到達之時 央處理單元2 〇 〇。 。二距離估測被送至中 在程式存儲器220中儲存了一读炉心 由中央處理單元2〇()執行以將得自輸^ #之演算法，並 結果轉換為位置及方向估測。中央严^界面2 10之量測 固定之資料儲存器240，此儲存哭包处早凡20 0亦耗接至 位置之對應資料及超音波測距單。元丨^°預設超音波信標 理器130亦包括一通訊界面26〇 麥克風位置。處 至其他裝置，如顯示裝置28〇，用以、处理單元2 0 0耦接 置及方向修正其顯示值。 又象追蹤裝置100之位 本系統之操作請參閱圖3， 厂列連接之空心圓及箭頭房間之上視圖。 t間點上之實際位置及方向。S3裝置1〇〇在每-=4間點上所得到之慣性量 =之I測結果及在 代表追蹤裝置1〇〇在第一時間、、'α果，貫心圓及箭頭312a 測。在下—個時間點，追蹤‘ j兹位置及方向之估 依據新的慣性量測結果將移動至位置31〇b，且 驟在下-個時間點又重複二，：測更新為312b。這個步 人传到產生實際位置310c及

五、發明說明（8) 估測位置3 1 2 c。 令，Γ』ίί置310b後’追縱裝置100送出-紅外線指 所示。：接換器122之位址值，如虛線32。 赶立、& π ^ 外線私令後，超音波轉換器1 22送出 時=^壯晋^置1〇0 °依據到達時間，當波形㈣到達 衣置1 0 0估測本身位置為3 3 2。 在下 個時間點時，追縱裝詈1 0 0首4^曰 ==;r,r:r得之超音波來回時間，追縱 置31〇e及估測312d,。使用在實際位 一# Ϊ 之慣性量測結果重複此步驟。 可以一把】t〖貝，里測及超音波量測在每一個時間點都 個時間點卜，，儘管超音波量測之頻率可以較少。在每一 # 位置及方向都被更新。超音波脈衝可以經由 社灸關同/ k 捉択位罝及方向之相關資 42〇a" 1 量測單元140包括三個角速度感 c，及二個線性加速度感測器4 1 0a-c。這些感測i係 番古》V 土》 * ....... _ 使用：=排列之麥克風提供位置及方向之相關資訊。

口月翏閱圖 4，憎}士 罢：B,l H2 二 1 >ί Λ h K . H .、几“ _ 個線性加速度感測器4 1 0a-c。這些感測器· J 千《丨；Τ'凹疋於追蹤裝具 食可米稱肀。每一個加速度感測器提供—約斑在相 成比例之信號，而每-個角速度感測11提 ’、相對軸上轉速成比例之信號。 作，3 ::性量測單元140之方向改·，如加速度信號等 口 k i /、在该房間之固定參考架構之方向改變對應。慣杉

497967 五、發明說明（9) 量測單元140亦包括一信號界面43〇， 測量儀及角速度感測器之信號， 個加速度 ，此資料流將加速度及角速二 二=下將說明的，加速度及角速度信號是‘處 可此含有額外的偏值及大小誤差。 ^的且 體之運動而不同。 W些决圭及偏值會因物 請參閱圖5，每一個超音波量測 發光二極體5 1 〇，由紅外線信號產生 紅外線 生器川接受來自輸出/入界面 外線發光二極體510以送出該信號至一;^亚驅動、、工 標122。欲量測其距離之超音波产祐二個超音波信 川中。每一個超音波信標i 2 2包= 入f列信號 =在Ί則距早元110與該超音波信標間有足夠短且 ^早域之路徑，該感測器540則接收紅外線信號， 解碼器542解碼：此解碼後之信號包括了由 後^ : Ξ ^ =运出之位a f #。控制電路560接收解石爲 號，並判斷超音波信標是否的確被指定位 立、，則通知一脈衝產生器5 52提供一信號至一超 曰波轉換$550 ’產生-超音波脈衝。此脈衝通過空中至 超曰m距單元11 〇 ’由一麥克風52〇接收，此麥克風 則，送一相對之電子信號至一脈衝偵測器522，產生一邏 輯^號’顯不脈衝之到達。此脈衝偵測信號被送至輸出/ 入界面2 1 0。如以下將述，信號之來回時間並無法精確地 估測距離。誤差來源包括偵測脈波時之時間誤差、因氣溫

第12頁 497967 五、發明說明（ίο) :化造成之聲波傳遞速率變動，及 音波在不同方向上之傳遞不一致。、9波信標發出之超 輸出/入界面2 1〇包括邏輯元件電路 輯陣歹Ο,如圖6所示。—慣性量測單元—可程式之邏 受來自慣性量測單元14〇之序列式編碼之貝枓緩衝器630接 料413，並輸出六個加速度及旋轉量測 ；及角速度資 單元20 0。輸出/入界面210亦包括一俨；f 31至中央處理 中央處理單元20 0提供欲量測之超音^ =址緩衝器61 0。 位址緩衝器6 1 〇將位址儲存並提供序列之位址。信標 —個超音波測距單元110。每一個超音、^之位址值至每 位址值之同時，三個計數器62〇a_c被重、’距單兀1 10送出 固定之時脈頻率增加。當每一個超音波始從零以一 來自信標之超音波脈衝時，相對之脈衝偵γ Π 〇偵測 對之計數器而停止計數。中央處理單元便取^號被送至相 為對超音波從超音波信標至每一個超音 侍此計數值做 時間之測量值。請參閱圖7a_b，追蹤裝置距曰單元之來回 在房間之參考架構中之位置，如710所示传出其本身 方向上分別具有自軸710起之位移成份（ΓΝ(η)，^⑷

E(束）及D(下）。位置r⑷73〇係一向量，此:不為N(北）、 方向 K 合 θ 七 ώ 紅 71 η 士》μ / I 在 N ' E 及D (η) 追 r 縱裝置1 0 0亦決定了其方向 口月參閱圖7b ’方向係以㊀二（ψ，㊀，φ)τ來 個E u 1 e r角係以3 X 3之方向餘弦矩陣，cbn (㊀\ 這 矩陣將以物體參考架構座標表示之向量藉由^代表’此 切勒作進行轉 z、y及χ軸為中心之yaw、pi tch及ro 1 1移動叙从

第13頁 497967 五、發明說明（11) 換。此方向餘弦矩陣可以被定義為 c(0_)= 1 0 0 0 0 cos ψ -sinΨ sin^ cos Ψ cosS 0 -sinG 0 1 0 sinΘ 0 cos Θ cosi> -δίηφ είηφ cos® 0 0 0 0 ^ 、 /

Cbn之上標及下標代表此矩陣輸入一在b參考架構中之 向量而產生一在η參考架構中之向量。 請參閱圖8，慣性感測器80 0，包括旋轉感測器42〇a_c 及加速度感測器41 Oa-c，提供慣性量測信號至一慣性追縱 為8 1 0。慣性追蹤器8 1 〇對圖8所示之信號流執行一不連續 時間之估測。慣性追蹤器82 0修正角速度信號亙以計算量 :結果中之偏冑。在此例中’只修正了一正向偏值“。 ::偏值，如乘積誤差（即一比例上之誤差），及因^而 之块差亦可以被修正。加速度測量儀補償83〇以類似 :方j修正在加速度信㈣中之正向偏值^。有數個來 ^包括偏值項^及-，係使用超音波量測進行/ 信號= ΐ:8。40在將此^^^ 示此方HLr::方向積分使用—方向餘弦來表 y⑴、不連續時間執行之連續微分方程 % Ct)s( ω(〇)被用來以固定速率Γ 至200之間）更新古Α3 ^ u疋浞半（通常在母秒1〇〇 記改變後，此不〗、玄矩P。將-不連續時間系統之標 之方程i實績時間之方向餘弦矩陣之更新係以以下

第14頁 497967 五、發明說明（12)

Ck = Ck-i(I+ 雙 S(跑)+上尝^S(W ) 其中，跑=At，袍=||跑||

0 -δθζ 且 S(_ia) = δθζ 0 δθ X -όθ όθ 0 k y X 為g Θ之斜邊對稱矩陣。S ( g Θ)滿足以下式子： S( _i_^)2= 5 Θ2Ι-g Θτ 為了確保Ck是真正的方向餘弦矩陣，在每一次移除數 值或估測誤差後，其列均被正交化。 依據被追蹤之方向餘弦矩陣Ck，座標轉換8 5 0接受偏 值修正後之物體參考架構中之加速度信號並輸出一 navigation參考架構之加速度信號，依據以下之式子： 旦k(n) = Ck(互k_(b)-_^k(b))+ (0, 0, -g)T 雙重積分8 5 0依據以下式子計算速度及位置： 且 %-ι(η)+%(η) 2

At ί

At-l·

At2

Euler角計算870輸入方向餘弦矩陣並輸出相對之 Euler角。慣性追蹤器81 0之輸出為（旦，r(n))T。慣性追蹤器

第15頁 497967 、發明說明（13) 維值所組成 之狀態包括一1 5維之向量，由5組 圣二（豆，发，工⑷，v(n)，a(n) )τ 述，慣性㈣器川接收來自超音 mi信號[X，其用以修正方向、速度及位置、、σ 數。、’更新疑轉儀及加速度測量儀偏值修正元件之各項參 81 0 圖9，一信標序列器9 1 〇接收來自慣性追蹤哭 ‘序列益910使用一超音波信標 ΐ::!:應915決定每一次時間點應觸發之信標，以產 靠；二二’貝1距結果。舉例來說，信標序列器9 1 0選擇-最 行^严=置之信標，並在每一時間點上在這些信桿中進 在ίί序3著位置估測之改變近之信標組合亦改變。 ===序列器91〇依序觸發每一信標後’相對之超音波^ 刿麥克i被追蹤裝置偵丨則。每一個脈衝產生一個相對-偵 個測距測！:ΐ,。★此例中’每-個脈衝產生-組三 元1丨0 Γ之麥克測距結果係分別來自三個超音波測距單 之過Ϊ參’、測距結果920對應於接收超音波測距估測 置匕⑷ 關之測距結果參數係指定位址之信標位 測以偵測脈衝之麥克風位置心〕、測距估測七及偵 注音7衝之時間仁，％係被用以修正量測中之延遲。可以 Μ到’如果位置估測沒有誤差且測距估測也 4，測距估測將會是： 正確 dr=l b(n)-([(n)(tr)+CbHtr)m ⑻丨丨 五、發明說明（14) 由此等式所得到之誤差值 性追蹤器8 1 0之輪出。 、參數值並做為慣 一補償式Kalman濾波器被追 蹤之位置及方向估測，係藉由逐 用末改善破追 被追縱之量值更新。請參閱圖/此方：：：=果將 件。隨著慣性追蹤器810更新呈 ：括兩種相關之元 1010維持一x中誤差 綠』出Z —Kalman估測器 來說，在慣性追縱器81Q ^ ：： ί Zv：：lance)矩陣P。舉例 此共，矩陣p會對應於-永遠增加之誤差。的以下， 第一個元件是一 Kai man更新 ^ 果920之資料並使用此結果決m ’接党來自測距結 會送回慣性追縱器810而被 ：積=測h，此值 音，更新二更 出决差圣中一新的估測共變矩陣p (+ ’ <、、、， ^

Ka 1 man估測器丨〇丨〇。每一 車亦被送回 追蹤器810之輸出。一連串H曰立波/測部份地修正了慣性 在一定值之下。 串的超曰波更新確保了誤差保持 慣性追蹤器8 1 〇係一非蝮柯 考 濾波器用以建構一由言斯^: 處理為，所以將1^1·11 恰當的。“使用被：為 '二：f】動之：線性濾波器是不< 濾波器，便可以使用一^ n濾波器」（EKF)之 器81 〇之輸出x中的誤差僂^怨糸統模型來模擬慣性追蹤 a f中的為差傳遞。由EKF模型產生之誤差為 ^ φ 0 As(n\ Ai(n\ Aa(b)y 、中具有相對於慣性追縱器輸出向量之]份。可以注意 ^/^67 五、發明說明（15) 到’块差項係以物體座標系統來表示，而非 差標系統’而其他成份則直接相對於慣㈣ ㈣換矩陣Λ;及此線， 驅動此模型。二：：動雜訊之共變矩陣’此雜訊係用以 樂。在、、〃右x述兩種矩陣均受慣性追蹤器之輪出之# 备在，又有任何量測結果的庐：兄下，龟呈★ τ出之衫 然而，誤差之址礅s n 〇1之平均值是零。 之，？加。誤差傳遞之線性 战-F(U仏1 +見i ^ 係得自用於追蹤器81〇中更新方 且對應於以下之誤差傳遞方程·· 主擾亂分析， 〇k —1 —Cbn δ ok-1 δ wk== δ ook-l ^=^r+At_^Vk.1-1/2At2{Cbnda )(b) s(UUk，) + (〇, 〇, 一g)”} ^ak(b)-—、丨）[D(〇, 0,-g)T] 雜訊Wk之共變4被假設為diagonal。此妓 係侍自提供給慣性追蹤器8 1 G之慣性量^ :陣之en t r 1 e丨 源，包括正向偏值誤差、比例誤差、咸、中已知之誤差來 準誤差及來自感測器本身之信號雜訊4巧與物體軸之到 :d iag( σ2 影響之個別變異數（variance)如下：叉^性追蹤器輸出 σ2φγ η - a ^ ^ r 〇 Φχ， σ 〇Λ) πτ 2 其中’個別的變異數如下 497967

五、發明説明（16) σφ x=GyroScale ωχ AtfGyroAlign( ΔΗ-GyroNoise-At ^y=GyroScaleo?7AtH-GyroAlign(a?x+^2)AH-GyroNoisevSt σ*Φ 2=GyroScale ω2ΔΗ<57Γ〇Α^η(ωχ+ω^) △trfGyroNoisevSF a*^=GyroBia^hangeRate At w〜=0 0^=Ααοβ13ο&1β ax At+AccelAlia^) ΔΗ-AccelNoisevSt Ac c elS c al e Qy Δΐ+Ac c elAli gti(ax+aj ΔΗΆ c c elNoi S evSF C^2=AccalScale At+AccelAlig^a^+a^ &trfAccelN〇isevSt c c elBi as Chang eRate At 其中，GyroScale 、AccelScale 、GyroAlign及AccelAlign 分別對應於用以補償儀器誤差之校正不確定度。一般說 來，一非d i agona 1雜訊共變數可以使用。 請參閱圖11，Kalman估測器101 0具有兩級。首先是一 誤差線性化級11 10計算Fk及Qk。之後，一共變傳遞級1120 在每一時間點上藉由使用一Kalman濾波器共變傳遞方程重 複地更新誤差共變值： 當Kalman估測器10 10接收一更新後之共變P( + )，係由一超

音波測距所產生，此更新後之共變便會取代現在之誤差共 變p 〇 請參閱圖12，Kalman更新器1 02 0接受測距量測920之 輸出’亦接受來自慣性追蹤器8丨0之位置及方向估測、來 自Ka 1 man估測器1 〇丨〇之位置及方向估測誤差共變，並計算 出一誤差估測及使用此誤差估測得到一更新後之共變。

1 I 1 1 I S 1 E 1； 1 1 i I 第19頁 497967 五、發明說明（17) "〜-〜· ----------

Kalman更新器1〇2〇之第一 、 離與量測距離間之差係P級為量測剩餘計算1210。期望距 ddr = dr- | 可以注意到，通常一測2 bn(tr)I(b)l丨 測到時便使用。為了將里測結果有時會在其一開始被偵 在聲音脈衝到達時之位匕延遲考慮在内，使用了追蹤裝置 用時之位置及方向。現t及方向估測，而非在量測結果使 估測被用來做量測時間^ ί f、方向及直線速度及角速度 為了使用KalmanJ^2以決定^)及^>)。 觀察方程式· , &，此剩餘计异係使用一線性 5jc + v 矩陣，，、)1位置及方向 bDm E~b^D'hy%<I)mE bi V%七〇 〇 〇 4 4 ，…

rN"+tn^rE-hnE-bE 《 ~ϊ~ 交異數R ( b，dr，]5 )係用以模擬超音波測距中不同之現 象 舉例來說’當距離之增加時，由於脈衝的分散而使得 脈衝偵測更形困難，且此時使用一較大之變異數來模擬相 關之測距誤差。此變異數1?(^，1(1,，！0)具有以下之型式：

第20頁 497967 五、發明說明（18) R二 a2u+ σ、· 且 ^^^NoiseFloor+NoiseScale ά1

Vy，νζ，0，0， 以及 a2^(k △ t-tr)Hk( ωχ，6Jy，ωζ，0, 〇, 〇, νχ 〇, 0, 〇, 0)τ

Hk之第一、二項亦可以設定為零，以進行加速度測量 m如果需更精確）。如果第三項被設定為零 角之飄移修正將會花更多時間但更精確。

Kalman更新器1〇2〇包括一量測結果接受/拒絕級 1 23 0。接受/拒絕級123〇輸入一量測剩餘值，及其管 出之變異數R。如果量測剩餘值大於一預設倍數之標"r 離值，則此量測結果將會被視為錯誤而拒絕。否則$其將合 被進行一步處理而計算出狀態誤差估測立^。藉由使用、曰 Kalmari濾波器更新方程，KaUan增益計算124〇計算 Kalman增益為： ^ K = PkHk'(HkPkHkHR)-丨 =著’誤差估測器1 250計算出誤差 變更新器1 260計算更新後之誤差共變為： P(+)=(I-KH)Pk 立之成份接著均被用來争紅 芬XV 口η、， 木吏新慣性追蹤器81 0。得出之δω 及立^ )分別被送至旋轉儀值 偏值修正830(圖8) ’复中^值修正82〇以及加速度測量儀 mv⑷及<5r⑷被送、至雔匕:與現存之偏值參數相加。得 現在π及_r-T估測才目加:分860 (圖8)，其中它們與 式子更新· 取後，方向餘弦矩陣依據以下

497967 五、發明說明（19)

Ck -（I-S(企））Ck 並重新正交化。 安之獨立的2 L超音波信標陣列12 〇包括排列成一規則圖 ί;皮2請r舉例來說，信標可以被排列 於硬體設備，，俨吸之。距且精確度在3—以下。受限 同之位址值。因°:有一有位址值’此例中為8個不 址時，多U 當追縱裝置送出—紅外線指令至-位 2標是用來執行挪距量測。然〒：二有取近 ::-個紅外線指令脈衝偵測電路可m:能回 了避免這種提早觸發的問題，脈=衝到達時間尚早。為 ，脈衝會到達時間被觸發。器如（圖5)只在一 -人脈衝的反射而誤觸發。°避免被其他信標或前 在就明追縱及Kalman更卑片4。^ ::是已知的。但並不是每次，最襖之位置及方向 、得演算法。超音波信標之有=二、’且必需使用一自動 演算法之基礎。一開始，追蹤^ ，被用來估為最初取得 並星項彳最近信標之距離。然後，田 毛有位址值之信標 測距量測決定。追蹤單元包括—彳四個信標之位址值由 =信榡之位址及位置。這些信標之1地圖’此地圖包括所 標之位址值將可能的位置限制在彳列方式使最近四個信 ，近之信標產生模糊不清之情形，】==小空間中。如果 來估三角量測以解決不清之問顳 m仏‘之真正距離被用 、攻初方向係依據相關對

第22頁

&、發明說明（20) 麥克風之測距量測 所 先，使用 有的追蹤程序可以^ 用上述之太、土 由圖1 3之、、六 流程便開始進入每亡：:;；初位置 間點後（步驟1320) ^之步驟循環。在二=_)二 且追蹤變數x及誤差結果便被接收（步驟133〇人）了 驟1 3 4 0 )。如果、梦女 使用慣料旦、目丨丨处里丄 合被用炎^曾有尚未處理之/加以更新（步 誤差更新d 波測距結果，此結果 驟136〇)。誤差更新及新^ :及更新後之誤共變P⑴（步 追蹤器及Kalman估制^、铁i共變之後被用來更新慣性 時間點上進一步進::以步驟137〇)。然後，決定是否在此 後在一個時間點2 =測距。每個脈衝之三個測距結果完成 間點將處理之測距其中一㈤’可能會產生下-次時 上都不需要再番…Γ果的囤積。因此，在下幾次的時間點 和立 设新的測距量測。接著，由於將下一次之 ‘否應^ Ϊ =订之預期時間考慮在内，決定在此時間點上 選…;=出=紅外線指令至一信標（步驟1 380 )，以及是否 僮i、、，:個信標位址值（步驟139〇)，如果是，紅外線指令 循运 步驟1 3 9 5 )。接著，整個步驟又從步驟1 3 2 0重新 循％，等待下一次時間間隔之開始。 _ 士種/、他的了行方法也可以使用。在此實施例中，每 固4間點只使用一個測距量測。也可以在每一個時間點 之^ 有可進行之量測’如果中央處理器1 3 0具有足夠 。异此力。這種方法可以從步驟丨37〇開始循環至步驟 〇 ’值到所有量測都已進行。此外，不只是使用Ka 1 man

第23頁 7 五、發明說明（21) 依序將測距量 更新方程以更 耵不只是等待 蘇與慣性追縱 上述之流 傾角計可以用 方向飄移。此 二個方向之旋 叶量測進行。 但卻不需要易 個以上之麥克 值進行更新 新向量及雜 下一個測距 器之更新同 程可以與其 來提供對擴 外，不只是 轉角，也可 如此，有些 受磁場影響 風以提供更 ，可以在單 訊。另外， 量測，測距 步 〇 他的量測模 充K a 1 m a η濾 使用三個或 以只使用兩 飄移修正可 之指南針。 多的旋轉自 一步驟中使用類似之 在下一時間點到來之 量測可以整合，而不 式合併。舉例來說， 波器之量測，以修正 更多個麥克風來修正 個麥克風並配合傾角 以以傾角計為基礎， 也可以使用遠超過三 由度。 不同之 超音波 置。可 會已移 於補償 信 或天花 可架在 其位置 及圖形 及位置 或減少 另一種與將信標置於定點及麥 方法，稱做「〇utside_in」。 脈衝及一麥克風之座標陣列感 以注意到’在一脈衝到達一麥 動至一新的位置。這種量測上 測距ΐ測中的方法類似之方式 才示1 2 2不需置於一平面陣列中· 板’或在環境中其他的支持物 吊燈上。信榡的數目可以配合 則可以依據—些原則來選定， 排列上的考量，且信標地圖可 來设疋。一信標群中的信標數 ，只要彳§標地圖能夠隨時保持 :衣且 此時之追蹤裴置提供 測一追蹤裝置之位 克風時，追蹤裝置將 的延遲必需以上述用 進行補償。 >它們可以置於牆上 上。舉例來說，信標 使用者需要調整，而 如可用架設點之選擇 以配合所選定之數目 目可以隨使用者增加 更新即可。

497967 五、發明說明（22) 自追蹤裝置到 傳輸之方式發送， 以以有線之方式連 使用「i n s i d e 個以上之物體。請 縱其位置。一第一 信標1 4 1 〇， 聲音脈衝而 脈衝，追縱 向。此例中 測單元，並 了在相對位 標置於~第 追縱第二襄 請參閱 互追ί從網 置之位置， 以藉由一輕 網路。 在上述 正確無誤的 排列誤差會 的說’信標 態中’且來 被以特 使追縱 裝置可 又包括 輸出其 置及方 二物體 置之相 圖 14b， 路」。 包括固 接追縱 之實施 由於 被重複 之排列 每一

第25頁 信標之指令信號可以使用其他非紅外線 如射頻、可見光或聲波。追蹤裝置亦町 接至信標。 — outside-in」之方式可以同時追縱兩 參閱圖14a，追蹤裝置1〇〇如之前一樣追 追蹤裝置1400包括三個可位址之超音波 定之方式排列。藉由觸發信標1 4 1 0發送 裝置1 0 0上之超音波測距單元1 1 〇接收此 以得到第二追蹤裝置之相對位置及方 一固定於追蹤裝置1 4 〇 0上之第二慣性量 慣性量測結果至追蹤裝置1 0 0，其提供 向之估測中的準確度。如果只有一個信 上’僅需使用超音波測距量測，而不需 對方向便可以得到相對之位置。 可以使用一由多個追蹤裝置所組成之 =些追蹤裝置追蹤其個別相對於其他裝 疋^標及其他移動中之被追縱物。這可 衣置之通信系統來達成，如一射頻區域 ^中’信標陣列之「地圖」是被假設為 j距結果中包S 了足夠的資訊，信標的 I估測及更新，因而增加準確度。具體 =差可以包含於擴充Kalman濾波器之狀 信標之距測結果可以用來估測排列誤 497967

模式，藉由來自房間 以得到信標之位置。 之位置使用，或做為 之初步估測使用。在 於房間中，而不需精 五、發明說明（23) 差。可以使用一分離式初步自動對應 中一個以上之測距結果及三角計算可 這些自動決定之位置可以被做為已知 將進一步使用Ka 1 man濾波器進行更新 這種方法中，信標可以不規則地安置 確地排列。 工述i延蹤万法可以有好幾種應用。 追縱裝置耗接至一頭頂顯示器。請彖 器可以使一使用者能夠直接看::圖3;;;顯示 1 520，如一工作件。顯示器151〇使用工作件丁體 置，將資訊再加入使用者之工作件 R ^ 位 在對大型裝置使用一有線操控時說’ 可以用來在—_擬者^ j 士貝汛。—類似頭頂顯示器亦 不只匕實際之，者視窗中加入晝晝*，而 空白背景Γ通作二V通常會將-攝影主題賴放置在一 加入，而完成背景佥面二二電子儀為將其他的晝面 機1610移動時，北。廷種技術中存在的因難是當攝影 動作。藉由將追：二面ηη也力要跟著改變以襯托出攝影機之 之位置及方向便被4。00木5又於攝影機1610上，攝影機 收攝影機位置及 ^縱j而背景晝面便可以自動地由一接 可以建構啟六M i向改變之晝面處理器來修正。這種方法 子；旦面處理器之大型「virtual set」，因

497967 五、發明說明（24) 而可以使用多個改變中之攝影角度。 再應用疋關於車辅中一元件動作之 s動撞擊測試中。請參閱圖17，假人172。在撞擊車= 中之動作可以使用追蹤裝置100來追蹤。另外，一第-物 體，如防火牆上之一點可^ , 弟一物 「inS1de —_lde-ln二一領外的信標173〇 藉由 中假人動作及車輛中相對於假人的某一點之追縱進仃車輛 其他的應用，如自叙“ — _ 目動航仃、物貢及人員之追蹤、娱 樂、全物體追蹤及武器追蹤等等。 、 說，$ t，：：：2他的方法來進行慣性追蹤。舉例來

Euler角\疋四ΛΓ/ 弦矩陣來進行方向積分，使用 線性誤差傳遞系統矩陣及驅動” //合可：注意到’ 法之影響而有不同。同時Kal :;:會受特疋追蹤演算 變，如丄n日守，Kalman濾波器之狀態也可以改 度測旦ΐ可=其他項目。一個例子卜不只是追蹤加速 (即ϋ之偏值，同時也追縱加速度信號之乘積偏值 ρ比例誤差）、對準誤差及聲波速度。 位、t發明亦可以使用其他的測距量測法’包括聲音相 射頻或光飛行時間、射頻或光相位。 之鲈^發明亦可以使用其他的方法來融合慣性及聲音量測 或H，而不只是*^_濾波法。舉例來說，神經網路、 ^ 、糊邏輯系統、最佳化法都可以被使用。 在上面之祝明中，只有8個不同的超音波信標位址被 。除此之外，每個信標可以個別地被位址化，或可以

第27頁 497967 五、發明說明（25) 使用更多共享的位址。如果信標個別地被位址化，慣性取 得可以藉由使信標回應位址組或是在取得相位中分給個別 信標之序列指令而進行，如此追縱裝置可以藉由依據追蹤 裝置已知之信標地圖先找出範圍中之一信標再尋找更近之 信標而「z e r 〇 - i η」至其慣性位置。這種方法亦可以在追 蹤區域是由數個方間所組成時使用。在一開始，追蹤裝置 所處之方間被決定，之後在房間中之位置便可以找到。 本發明雖已以較佳實施例揭露如上，但其並非用以限 制本發明。任何熟悉此技藝者，在不脫離本發明之精神和 範圍内，當可做些許之更動與潤飾。因此本發明之保護範〇 圍當視後附之申請專利範圍所界定者為準。

第28頁

Claims (1)

1. 497967 六、申請專利範圍 1. 一種移動追蹤方法，用以追蹤一物體之運動，該方 法包括： 提供兩種該物體運動之量測法，其中一種包括聲音量 測法； 依據該兩種量測法中之一量測法，將一該物體之位置 或方向之估測加以更新； 依據該兩種量測法中之另一量測法，將該估測加以更 新。 2. 如申請專利範圍第1項所述之方法，其中該兩種量 測法之一為聲音測距法。 3. 如申請專利範圍第1項所述之方法，其中該兩種量 測法之一為慣性量測法。 4. 如申請專利範圍第1項所述之方法，其中該估測係 方向估測。 5. —種移動追蹤系統，用以追蹤一物體之運動，該系 統包括： 兩個感測系統，分別提供兩種該物體運動之量測法， 該兩種量測方法中之一為聲音量測法； 一處理器，搞接至該兩個感測系統，並依據該兩種量 測法中之一量測法，將一該物體之位置或方向之估測加以 更新，亦依據該兩種量測法中之另一量測法，將該估測加 以更新。 6. —種追蹤裝置，包括： 一感測系統，包括

   第29頁 497967 六、申請專利範圍 一慣性感測器； 一組或一個聲音感測器，緊密地耦接至該慣性感測 為 ， 一處理器，執行以下功能： 接收該慣性感測器之量測結果； 使用接收之該慣性感測器之量測結果將該感測系統之 一位置估測及一方向估測加以更新； 從複數聲音參考裝置中擇一； 接收一聲音測距量測結果，該聲音測距量測結果與該 感測系統及所選擇之聲音參考裝置間距離有關； 使用接收之該聲音測距量測結果將該位置估測及方向 估測加以更新。 7. 如申請專利範圍第6項所述之裝置，其中該感測系 統包括一發射器，用以發送一載有該被選擇之聲音參考裝 置識別之控制信號，且每一個聲音感測器包括一麥克風， 用以接收一來自該聲音參考裝置之聲音信號。 8. 如申請專利範圍第6項所述之裝置，其中該組或該 個聲音感測器包括兩個或更多個聲音感測器。 9. 如申請專利範圍第6項所述之裝置，其中 使用接收之該慣性感測器之量測結果將該位置估測及 方向估測加以更新之動作包括將該位置及方向估測中一不 確定度加以更新；以及 使用接收之該聲音測距量測結果將該位置估測及方向 估測加以更新之動作包括決定一該測距量測結果之不確定

   第30頁 497967 六、申請專利範圍 度，並使用該測距量測結果之不確定度將該位置及方向估 測中之不確定度加以更新。 I 0 · —種移動追蹤方法，用以追蹤一物體之運動，該 方法包括： 從複數參考裝置中擇一； 發送一控制信號至該被選擇之參考裝置； 接收一來自該參考裝置之測距量測信號； 接受一測距量測結果，該測距量測結果與至該被選定 之參考裝置有關； 使用該接受之測距量測結果將一該物體之位置估測或 方向估測加以更新。 II .如申請專利範圍第1 0項所述之方法，其中更包括 依據測距量測信號之飛行時間決定一測距量測結果。 1 2.如申請專利範圍第1 0項所述之方法，其中發送該 控制信號之動作包括發送一無線控制信號。 1 3. —種電腦軟體，儲存於一電腦可讀取之媒介中， 具有指令使一電腦執行以下之功能： 從複數參考裝置中擇一； 發送一控制信號至該被選擇之參考裝置； 接收一來自該參考裝置之測距量測信號； 接受一測距量測結果，該測距量測結果與至該被選定 之參考裝置有關； 使用該接受之測距量測結果將一該物體之位置估測或 方向估測加以更新。

   第31頁 497967 六、申請專利範圍 1 4 . 一種追蹤系統，包括： 一聲音參考系統，具有複數聲音參考裝置； 一追蹤裝置，包括： 一感測系統，具有一慣性感測器及一組或一個緊密地 耦接至該慣性感測器之聲音感測器； 一處理器，接收該慣性感測器之量測結果，使用接收 之該慣性感測器之量測結果將該感測系統之一位置估測及 一方向估測加以更新，從複數聲音參考裝置中擇一，接收 一聲音測距量測結果，該聲音測距量測結果與該感測系統 及所選擇之聲音參考裝置間距離有關，使用接收之該聲音 測距量測結果將該位置估測及方向估測加以更新。 1 5 ·如申請專利範圍第1 4項所述之系統，其中該感測 系統包括一發射器，用以發送一載有該被選擇之聲音參考 裝置識別之控制信號，且每一個聲音感測器包括一麥克 風，用以接收一來自該聲音參考裝置之聲音信號，每一聲 音參考裝置包括一接收器，接收來自該感測系統之控制信 號，以及一聲音轉換器，送出該聲音信號。 ί

   第32頁