專利名稱：利用感測元件的定位與檢測的系統(tǒng)與方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種利用感測元件的定位與檢測的系統(tǒng)與方法，且特別是涉及一種利
用可互補的多種感測元件的系統(tǒng)與方法，其定位載體、預(yù)測環(huán)境特征物件的位置、檢測與追 蹤動態(tài)物件。
背景技術(shù)：
室外定位系統(tǒng)，比如GPS(全球定位系統(tǒng))，已廣泛使用于車用導(dǎo)航系統(tǒng)，以在室外 定出車輛或人的位置。但室內(nèi)定位系統(tǒng)至今仍有其問題無法突破，室內(nèi)定位系統(tǒng)的困難度 在于(1)在室內(nèi)時，電磁訊號易被遮蔽，無法接收到衛(wèi)星訊號；(2)室內(nèi)環(huán)境的變動性較室 外環(huán)境大。 目前來說，室內(nèi)定位的技術(shù)有兩種類型，一種為外部定位系統(tǒng)，另一種為內(nèi)部定位 系統(tǒng)。外部定位系統(tǒng)例如，利用外部的傳感器與機器人的接收器之間的相對關(guān)系去估測機 器人在空間中的位置。由內(nèi)部定位系統(tǒng)例如，在機器人上放傳感器，將所掃描到的數(shù)據(jù)比較 于其內(nèi)建地圖，進而估測機器人在空間中的位置。 外部定位系統(tǒng)的定位速度快，但需事先建好外部傳感器， 一但這些外部傳感器被 移動或被遮蔽，此系統(tǒng)將無法定位。外部定位系統(tǒng)如要用于大范圍，所需的傳感器數(shù)目與成 本將增加。 內(nèi)部定位系統(tǒng)的定位速度較慢但具擴充性，即使環(huán)境變動性大，只要仍有特征點 可供定位，內(nèi)部定位系統(tǒng)仍能定位。但其必需先內(nèi)建室內(nèi)環(huán)境地圖，才能進行定位。考慮實 時性的話，可以同時建地圖與定位。然而此技術(shù)所建出的地圖是靜態(tài)的，但真實世界是動態(tài) 的，所以，能達成動態(tài)環(huán)境下的定位與建地圖的技術(shù)是必要的。 對于動態(tài)物件的估測通稱為追蹤。多個雷達可檢測空中的移動物體，以判斷是不 是有敵機或飛彈來襲。目前此種檢測與追蹤技術(shù)已可應(yīng)用在日常生活中，例如動態(tài)人物監(jiān) 控或其它安全監(jiān)控應(yīng)用。

發(fā)明內(nèi)容
為達有效室內(nèi)空間定位，和改善視覺傳感器易受光線干擾而造成定位誤差的 問題，本發(fā)明目的是采用多重傳感器間的互補性，以達成估測空間中物體狀態(tài)的系統(tǒng)與 方法，本發(fā)明利用電磁波感測元件(electro-magneticwave sensor)、機械波感測元件 (mechanical wave sensor)或慣性感測元件(i證tial sensor)，計算器率模型的感測融 合(fusion)算法，定位出載體位置并估測環(huán)境特征物件在空間中的相對位置，以達到定 位、建地圖、動態(tài)物件檢測與追蹤。 本發(fā)明提出一種應(yīng)用感測元件的定位與建地圖的系統(tǒng)與方法，其妥善結(jié)合各種傳 感器的特性進而達到三度空間定位與建地圖的功能。 本發(fā)明提出一種應(yīng)用感測元件的動態(tài)物件檢測與追蹤的系統(tǒng)與方法，將多重傳感 器對物體感測的結(jié)果進行同構(gòu)型(homogeneous)交叉比對或異質(zhì)性(non-homogeneous)交叉比對，以檢測出移動中的物體并追蹤此物體。 本發(fā)明的一例提出一種感測系統(tǒng)，該系統(tǒng)包括一載體；一多重傳感器模塊，配置 于該載體上，該多重傳感器模塊感測多種彼此互補的特性，該多重傳感器模塊感測該載體 以得到一載體信息，該多重傳感器模塊亦可感測一特征物件以得到一特征物件信息；一控 制器，接收該多重傳感器模塊所傳來的該載體信息與該特征物件信息；以及一顯示單元，受 控于該控制器，以提供一反應(yīng)信號。該控制器執(zhí)行下列的至少一者該控制器將該載體定 位于一地圖內(nèi)，且該控制器還將該特征物件加入于該地圖中及更新在該地圖中的該特征物 件；以及根據(jù)該特征物件信息，該控制器預(yù)測該特征物件的一移動量，以決定該特征物件是 否為已知的，并據(jù)以修正該地圖及將該特征物件加入于該地圖中。 本發(fā)明的另一例提供一種載體定位與建地圖的感測方法，該方法包括執(zhí)行一第 一感測步驟，以感測一載體以得到一載體信息；執(zhí)行一第二感測步驟，以感測一特征物件以 得到一特征物件信息，其中該第二感測步驟感測多種彼此互補的特性；分析該載體信息，以 得到該載體的一位置與一狀態(tài)，并將該載體定位于一地圖內(nèi)；分析該特征物件信息，以得到 該特征物件的一位置與一狀態(tài)；以及比較一地圖與該特征物件的該位置與該狀態(tài)，以將該 特征物件的該位置與該狀態(tài)加入于該地圖中及更新在該地圖中的該特征物件的該位置與 該狀態(tài)。 本發(fā)明的又另一例提供一種動態(tài)物件檢測與追蹤的感測方法，該方法包括執(zhí)行 一第一感測步驟，以感測一動態(tài)物件以得到其第一移動量；執(zhí)行一第二感測步驟，以感測該 動態(tài)物件以得到其第二移動量，其中該第一感測步驟與該第二感測步驟彼此互補；分析該 第一移動量與該第二移動量，以預(yù)估一載體與該動態(tài)物件間的一相對距離；決定該動態(tài)物 件是否為已知的；若為已知，修正在一地圖中的該動態(tài)物件的一狀態(tài)，并檢測與追蹤；以及 若為未知，將該動態(tài)物件及其狀態(tài)加入于該地圖，并檢測與追蹤。


圖1顯示根據(jù)本發(fā)明實施例的利用感測元件的定位與建地圖的系統(tǒng)。 圖2顯示利用視覺傳感器來計算物件在空間中的位置的示意圖。 圖3顯示雙眼圖像投影示意圖。 圖4A及圖4B是依照本發(fā)明實施例的利用機械波傳感器來檢測載體與環(huán)境特征物 件間的距離，以推測載體位置的示意圖。 圖5顯示本發(fā)明實施例的定位與建靜態(tài)地圖的流程圖。 圖6顯示定位與建靜態(tài)地圖的應(yīng)用情境。 圖7顯示本發(fā)明實施例的對動態(tài)特征物件檢測與追蹤的流程圖。 圖8是對動態(tài)特征物件檢測與追蹤的應(yīng)用情境。 圖9顯示本發(fā)明實施例的定位、建地圖、移動物件的檢測與追蹤的應(yīng)用情境。主要元件符號說明
100 :系統(tǒng) 110:多重傳感器模塊 120 :載體 130 :控制器
140 :顯示單元110a、110b、110c :傳感器210 、220 :預(yù)處理211、221 :噪聲移除212、222 :光線校正213、223 :圖像矯正IN1、IN2 :圖像信息CM :相機參數(shù)矩陣230、240 :特征提取250、260 :圖像描述270 :相似度比對280 :3D重建510 570、710 770 :步驟610A 610C、810A 810C、910A 910D :特征物件920 :手
具體實施例方式
為讓本發(fā)明的上述內(nèi)容能更明顯易懂，下文特舉實施例，并配合附圖，作詳細說明 如下 在本發(fā)明實施例中，能妥善結(jié)合各種傳感器的特性進而達到三度空間定位與建地 圖的功能。此外，在動態(tài)物件檢測與追蹤時，利用多重傳感器對物體的同構(gòu)型交叉比對或異 質(zhì)性交叉比對，以檢測出移動中的物體并追蹤此物體。 圖i顯示根據(jù)本發(fā)明實施例的利用感測元件的定位與建地圖的系統(tǒng)。如圖1所示， 此系統(tǒng)100包括多重傳感器模塊110、載體120、控制器130與顯示單元140。
多重傳感器模塊110可測量外界環(huán)境或特征物件的電磁波信息(如圖像或其它 不可見的電磁波)、外界環(huán)境或特征物件的機械波信息(如聲納等經(jīng)由機械震動所產(chǎn)生的 震波)與載體120的力學(xué)信息(如位置、速度、加速度、角速度、角加速度)。多重傳感器模 塊110會將所感測到的數(shù)據(jù)傳輸給控制器130。 在圖1中，多重傳感器模塊110包括至少三種傳感器110a、110b與110c，其中，這 三種傳感器所感測的特性不同，且可彼此互補。當然，多重傳感器模塊iio可包括更多種的 傳感器，此皆在本發(fā)明的精神與范圍內(nèi)。 比如，傳感器110a用以感測外界環(huán)境的電磁波信息，其可為可見光視覺傳感器、 不可見光視覺傳感器、電磁波傳感器、紅外線熱傳感器、紅外線距離傳感器等。傳感器110b 用以感測外界環(huán)境的機械波信息，其可為超音波傳感器、超音波數(shù)組傳感器或聲納傳感器 等。亦即，傳感器110a與110b可感測外界環(huán)境中的環(huán)境特征物件與載體120間的距離。傳 感器110c用以感測載體120的力學(xué)信息，其可為加速規(guī)、陀螺儀、轉(zhuǎn)速計數(shù)組或其它可測量 物體力學(xué)的傳感器等。傳感器110a易受昏暗光線或無光源干擾，但較不易受物件的形狀影 響到測量結(jié)果。另一方面，傳感器110b雖不會因為昏暗光線或無光源而影響測量，但會受 物件的形狀影響測量結(jié)果。亦即，傳感器110a與110b彼此互補。頁 多重傳感器模塊110安裝于載體120上。載體120可為車輛、機車、自行車、機器 人、眼鏡、手表、安全帽、其它可移動的物體等。 控制器130接收多重傳感器模塊110所感測到的載體力學(xué)信息與環(huán)境感測信息 (至少包括外界環(huán)境中的環(huán)境特征物件與載體120間的距離)，以計算(預(yù)估測)出載體狀 態(tài)信息、預(yù)估出外界環(huán)境中的環(huán)境特征物件的特性(如其移動距離、移動方向等)與建立地 圖。更甚者，根據(jù)幾何表達式，控制器130換算多重傳感器模塊110所傳來的載體力學(xué)信息， 以得知載體120的狀態(tài)信息(比如，載體的慣性信息、姿態(tài)等)。此外，根據(jù)幾何表達式，控 制器130換算多重傳感器模塊110所傳來的環(huán)境感測信息，可得知載體的移動信息或是環(huán) 境特征物件的特性(如其位置等)。 控制器130以數(shù)字濾波器，如卡爾曼濾波器(Kalman Fiter)、粒子濾波器 (Particle filter) 、 Rao-Blackwellised Particle filtetr或其它類貝氏濾波器推算出 載體狀態(tài)，以輸出到顯示單元140。 顯示單元140連接于控制器130。顯示單元140在控制器的命令下，產(chǎn)生對外界 的互動反應(yīng)。比如但不受限于，顯示單元140所產(chǎn)生的互動反應(yīng)可為語音信號、圖像信號與 提示信號的至少一者或其組合。語音信號包括語音、音樂、預(yù)錄的聲音等。圖像信號包括 圖像、文字等。提示信號包括顏色、色彩、明暗、閃爍、圖形等。比如，當檢測到別臺車輛快 要撞到應(yīng)用本發(fā)明實施例的車輛時，顯示單元可發(fā)出警示信息(聲音等)，以警告駕駛。
在本發(fā)明實施例中，控制器130的狀態(tài)估測可用數(shù)字濾波器實現(xiàn)，如下式所示，其
中xt為此刻的載體信息(包含位置(x， y， z)，載體姿態(tài)(e ，小，uO與地標狀態(tài)(xn， yn))，
其中為上一刻的載體信息，ut為此時刻的載體移動感測信息(如加速度(ax， ay， az)、角 速度("x， "y， "J等等信息)，zt為此刻傳感器所感測到的環(huán)境信息(如zx， zy， zz))。
xt = f (xt—p ut) + e t
zt = h (xt) + S t 利用數(shù)字濾波器，可經(jīng)由迭代的方式算出Xt，根據(jù)Xt，控制器130輸出信息到其它 裝置(如顯示單元140)。 以下解釋傳感器在測量空間中物件幾何距離的概念與方法。 [OO57] 電磁波(可見光) 利用視覺傳感器，可通過圖像來建立在空間中的物件位置與環(huán)境信息等。以圖像 感測為基礎(chǔ)，定位真實世界的物件如圖2與圖3所示。圖2顯示利用視覺傳感器來計算物 件在空間中的位置的示意圖。圖3顯示雙眼圖像投影示意圖。 如圖2所示，假設(shè)已知相機的內(nèi)參數(shù)矩陣與外參數(shù)矩陣，而由內(nèi)外參數(shù)矩陣可得 相機參數(shù)矩陣CM (camera matrix)。對所提取的兩張圖像信息INI與IN2(可由兩個相 機裝置分別提取得，或利用一臺相機在兩個時間點取得)可選擇性分別施以預(yù)處理210 與220。預(yù)處理210與220分別包括噪聲移除(noise removal) 211與221、光線校正 (illumination correction) 212與222 ;以及與圖像矯正(image rectification) 213與 223。若欲施以圖像矯正，則必須知道基礎(chǔ)矩陣(fundamental matrix)?；A(chǔ)矩陣的計算方 式如下所述。 在圖像平面上的以相機坐標系表示的成像點可經(jīng)由相機內(nèi)參數(shù)矩陣轉(zhuǎn)換，以得到 此點以2維(2-D)圖像平面成像點的表示方式，意即
7
<formula>formula see original document page 8</formula>
其中，Pl與pr分別為真實世界的物件點P于第一張與第二張圖像的成像點，其以
相機坐標系統(tǒng)表示；而，,與A.分別為物件點P于第一張與第二張圖像的成像點，其以2-D圖
像平面的坐標系統(tǒng)表示；M工與Mr分別為第一臺與第二臺相機的內(nèi)參數(shù)矩陣。 如圖3所示，pJ勺坐標為(xl，yl，zl)，而pj勺坐標為(xt，yt，zt)。在圖3中，(^
與0t為原點。 又，Pi與Pr可經(jīng)由必要矩陣(essential matrix, E)轉(zhuǎn)換，必要矩陣E為兩個相機坐標系統(tǒng)之間的旋轉(zhuǎn)與平移矩陣相乘后的結(jié)果，因此，
p,7、/ =0
上式可改寫成(《》,.丫 0 再將Mr與E合并后可得 (M,7服,'=0 因此，令 F = Mr—TRSM丄—1 可獲得，,與-,之間的關(guān)系式如下 =0 因此，通過輸入兩圖像內(nèi)的已知數(shù)組對應(yīng)點，即可由上式求得基礎(chǔ)矩陣。矯正后的兩張圖像會具有平行對應(yīng)的核線(印ipolar line)。 其后，對兩張矯正后圖像施以特征提取(feature extraction) 230與240，以抽取出有意義的待比對特征點或區(qū)域。接著，利用圖像描述(imagedescription)250與260來簡化特征，使其成為特征描述子(feature descriptor)，的后，對兩張圖像特征施行相似度比對(stereo matching) 270，找出兩張圖像中對應(yīng)的特征描述子。 令Pl與K的坐標分別為l"VJ與lz/A/j ，由于圖像中存在噪聲，因此可通過解開在3D重建(3D reconstruction) 280中的最佳化問題<formula>formula see original document page 8</formula> 以估測空間中特征點P的世界坐標位置，其中，!V'T，n^'T，!V'T分別表示相機參數(shù)矩陣CM的第一，二，三列。如此，可得到載體與環(huán)境特征物件間的距離。
電磁波(能量) 通常，在室內(nèi)環(huán)境中有多種電器，而每種電器都會產(chǎn)生不同的電磁波。故而，可以利用電磁波能量而計算出物件(其會發(fā)出電磁波)與載體間的距離，進而得知物件的位置。首先，可以先用電磁波傳感器來測量各類電磁波的訊號波形、頻率與能量，可建出以下函數(shù) <formula>formula see original document page 8</formula>
其中E(r)為能量函數(shù)，K為常數(shù)或變數(shù)，r為載體與物件間的距離。經(jīng)由電磁波的能量大小估算出物件與載體的距離，如此即可算出發(fā)出電磁波物件的位置。其細節(jié)請參考
底下關(guān)于如何利用機械波來估計出載體與物件間的距離。 機械波(聲納) 超音波屬于距離信息(Range-only)傳感器，也就是說，其僅能感測到物件在某個距離內(nèi)，而無法得知物件的確切方位。通過分析機械波的能量大小，或分析機械波的發(fā)收時間差，即可估算出特征物件與載體的距離，而再利用載體移動前后的兩筆距離信息以及載體的位置信息，即可得知特征物件或載體的所在位置。 圖4A及圖4B是依照本發(fā)明實施例的利用機械波傳感器來檢測載體與環(huán)境特征物件間的距離，以推測載體位置的示意圖。 請先參照圖4A，假設(shè)物件在k時刻的位置為(Xl， Yl)， k+1時刻的位置為(X 2，Y2)，其中k時刻與k+1時刻相距At，而At為固定取樣時間。假設(shè)機械波傳感器在k時刻的位置為(al，bl)，在k+l時刻的位置為(a2，b2)。根據(jù)機械波傳感器在這兩個位置所檢測到機械波能量的大小，或發(fā)收時間差，即可推測出發(fā)出此機械波的環(huán)境特征物件與載體之間的距離rl及r2。 接著，以機械波傳感器的位置(al，bl)及(a2，b2)為中心，距離rl及r2為半徑畫兩個圓，即可得到如圖4A所示的圓A及圓B，其中圓A及圓B的方程式如下
2 圓B — "2)+ (7 _ ~)Z =《
二
.2
(2) 其中，圓A及圓B交點的聯(lián)機為其根軸，而利用上述圓A及圓B的方程式即可求得此根軸的方程式
.2 , 7一2 , —2 2乙2— 2.
r = _ (2"2 - )工+ ^ +- < - ^ - Y(2~—2、) (262—2W
(3)
(4)
接著，令圓A及圓B的交點(XT，YT)的關(guān)系為YT = mXT+n
將式(4)代入圓A的方程式(1):
(;- ^ )2 + + - A )2 二氣2
(w2 + 1);r^ + (2w" — 2m^ — 2a )JTr + (" _ 、 )2 +力2 —= 0
再令P = m2+l ， Q = 2mn-2mb「2a"以及i 二 (W — 、 )2 + af — f即可得
-g土yg2-徴;=
附(—g土Vg2一4/^)
IV =----h "
2尸
(5) 通過上述推導(dǎo)可得兩組解(XT，YT)。參考所測得電磁波的幅角，即可決定哪一組解
9才是特征物件所在的位置。 機械波收發(fā)元件屬于距離信息(Range-only)的傳感器，也就是說，機械波收發(fā)元件僅能用以感測載體在某個距離內(nèi)，而無法得知載體的確切方位。機械波收發(fā)元件以機械震動來產(chǎn)生震波，其比如為超音波、超音波數(shù)組或聲納等。為了用機械波測量載體的位置，在本發(fā)明實施例中，亦利用載體移動前后的兩筆機械波距離信息與載體的位置信息，將特征物件的位置簡化為兩個圓解共點，其求解的方式與前述電磁波傳感器相類似，故在此不再贅述。 力學(xué)感測元件(IMU): 力學(xué)感測元件用于測量動態(tài)(直線運動、旋轉(zhuǎn)運動等)的物體的狀態(tài)。通過演算機制，對測量到的動態(tài)信號加以解析，就可實時得到移動物體在三維空間中的各項數(shù)據(jù)，包含位置，速度、加速度、角度、角速度、以及角加速度。 現(xiàn)在說明力學(xué)感測元件的感測原理。在初始狀態(tài)后，可根據(jù)陀螺儀量得載體的三
軸角速度信息，經(jīng)由四元素算法(integration of quaternion)算出載體的三軸姿態(tài)角，再
經(jīng)由坐標轉(zhuǎn)換矩陣的轉(zhuǎn)換，以得到載體在世界坐標下的三軸速度。在轉(zhuǎn)換的過程中，可引入
加速度傳感器的信息，通過對時間的一次積分以及重力分量的消除，以得到載體的速度信
息。再通過濾波器得到載體在三維空間下的三軸移動量信息的預(yù)測值。 如果僅利用此種感測信息，由數(shù)學(xué)運算積分所造成的累積誤差，以及由傳感器取
樣造成的誤差，將導(dǎo)致估測值與實際值越差越遠，并且隨時間增加而發(fā)散。所以必須搭配其
它種類的傳感器，消除飄移累積誤差。之后，再以機率型濾波器估測物體的位置。 或者說，力學(xué)感測元件在感測時，所用的運算包括四元素(integrationof
quaternion)運算、方向余弓玄運算(direction cosine convert to Euler angle)、重力分
量抽離(separating gravity)運算、力口速度禾只分(integration ofacceleration)運算、速
度禾只分(integration of velocity)運算、坐標轉(zhuǎn)換(coordinate transformation)運算、
數(shù)據(jù)關(guān)聯(lián)(data association)運算、延伸式卡爾曼濾波器校正(extended-Kalman filter
correction)運算等。 現(xiàn)請參考圖5，以說明在本發(fā)明實施例中如何進行定位與建靜態(tài)地圖。圖6顯示定位與建靜態(tài)地圖的應(yīng)用情境。在圖6中，假設(shè)載體120處于動態(tài)(移動及/或轉(zhuǎn)動等)，而在外界環(huán)境中有多個靜態(tài)特征物件610A 610C。在此，定位是指對載體的定位。
如圖5所示，在步驟510中，提取第一傳感器信息，此第一傳感器信息用于感測載體120的狀態(tài)。比如，提取傳感器110c所檢測到的載體加速度與速度信息ut = [ax, t ay, ta + co + co + co +"|T。
"z， tx， ty， tz， tJ o 接著，在步驟520中，根據(jù)第一傳感器信息來預(yù)測載體狀態(tài)。其詳細方式如下。假設(shè)欲估測載體在空間中的位置為[xt，yt，zt， 9t， (h， Vj，其中，[O川]Xt = g(Xt—一t)+et
zt = h(xt)+St假設(shè)運動模型(Motion Model)如下
Xt = g(Xt—丄，Ut)+h
其中載體狀態(tài)為 Xt =Vx,t Ax,t YG,t Vy,t Ay,t ZG,t Vz,t Az,t e。,t e2,t e3,t]T， [X^ YG,t Zc,t]T為載體
10在世界坐標中的絕對位置，[、t Vy,t 、jT為載體在載體坐標中的速度，[Ax.t Ay,t 、,t]T為載體在載體坐標中的加速度，[e。,t ei,t e2,t e3,t]T為載體在載體坐標中的四元素(quaternion),Ut= [ax,t ay.t az.t "x,t "y,t "z,t]T為載體在載體坐標中的加速度與角速度。
要算出載體于t時在世界坐標中的絕對位置Bt，需要利用載體于t-l時在世界坐
標中的絕對位置、利用載體上的加速規(guī)與陀螺儀所得到加速度和角速度的積分信息，且利
用四元素把載體坐標信息經(jīng)由載體坐標轉(zhuǎn)換成世界坐標，而且，以上步驟在運動模型中完
成。矩陣運算如下 載體狀態(tài)的運動模型
0
0 '
0000
0 0
00
00
0.5/ 3l,2000000
000
00000000
wl2,
0
0
0000
0 0
0.5WI2,200
00
000000
000000
00000
—A/
0
0
0000
0 0
0.5 W00
0.5/ 23,200
0.5/ 13/2000000
00000000
0
1
0.5(2^, /0.5< ,.,Z
00000000
0
—0.5"
1
000000000
000000000
一0.5ffl〕,乂 —0.5"2.,1 0.5ov
-0.5(UZ,,/ 0.5 ,,/ 0
1
Km
《,-,l」
_JJ_
11<formula>formula see original document page 12</formula> 經(jīng)由以上的運動模型可算出載體在空間中的位置[Xe,t Ye,t Ze,jT、載體在載體坐標中的加速度[Ax,t Ay,t 、t]T、載體在載體坐標中的速度[Vx,t Vy,t V"]T與載體的四元素[x。,t ei,t e2,t e3,t]T。所算出的載體狀態(tài)會包含加速規(guī)與陀螺儀傳感器的噪聲，故需要修正其誤差。所以采用另外一個傳感器當做感測模型(Sensor Model)，目的在于修正加速規(guī)與陀螺儀估測出來的物體狀態(tài)。
感測模型如下
Zt = h(Xt)+St 若傳感器為視覺傳感器，感測模型如下<formula>formula see original document page 12</formula>
其中，[mx,ti my,ti mui]T為第i個內(nèi)建地圖的空間坐標，S c, t為視覺傳感器的噪
若傳感器為聲納或電磁波傳感器，模型如下
<formula>formula see original document page 13</formula>
其中s"為聲納傳感器或電磁波的噪聲。
接著，如步驟530所示，提取第二傳感器信息，此第二傳感器信息用于感測外在(室內(nèi))環(huán)境中的靜態(tài)特征物件。第二傳感器信息比如可利用傳感器110a與110b的至少一者，或其二者所感測到的信息。也就是說，在步驟530中，利用電磁波類型傳感器及/或機械波類型傳感器來感測出靜態(tài)特征物件610A 610C與載體間的距離。
接著，如步驟540所示，比較第二傳感器信息與內(nèi)建地圖中現(xiàn)有的特征物件的信息，以決定所感測到的靜態(tài)特征物件是否已出現(xiàn)在現(xiàn)有的內(nèi)建地圖中。如果是的話，則根據(jù)第二傳感器信息來修正載體的位置、修正載體的狀態(tài)、與修正已內(nèi)建的地圖，如步驟550所示。 步驟550的詳細說明如下。由以上感測模型可算出載體在空間中的位置，進而修正運動模型所估測的載體狀態(tài)，以估測載體狀態(tài)，其中可估測的載體狀態(tài)包含載體在空間中的位置[Xe,t Ye,t U與四元素[e。,t ei,t e2,t e^]T，若需要算載體相對于X軸的角度9 、載體相對于Y軸的角度V與載體相對于Z軸的角度小可由四元素換算而得，其公式如下
<formula>formula see original document page 13</formula> 以上的運動模型與感測模型可代入數(shù)字濾波器，即可估測載體位置。
如果載體完全不轉(zhuǎn)動僅移動時，則所估測的載體狀態(tài)僅為^= [Xe,t Ye,t Ze,JT ;若當載體完全不移動僅轉(zhuǎn)動時，所估測的載體狀態(tài)僅為^= [e。,t ei,t e2,t e^]T或經(jīng)由轉(zhuǎn)換后的Xt二[e v小]T，以上兩種例子皆在本實施例范圍。 如果步驟540的判斷結(jié)果為否，則根據(jù)第二傳感器信息，以增加新的地圖特征于內(nèi)建地圖，如步驟560所示。也就是說，在步驟560中，將所感測到的靜態(tài)特征物件當成新的地圖特征，以加入于現(xiàn)有的內(nèi)建地圖中。比如，比較后結(jié)果發(fā)現(xiàn)特征物件610B并未出現(xiàn)于現(xiàn)有的內(nèi)建地圖中，則可將特征物件610B的位置、狀態(tài)等加入于內(nèi)建地圖。
接下來，則說明本發(fā)明實施例如何應(yīng)用于的動態(tài)特征物件的檢測與追蹤。圖7顯示本發(fā)明實施例的對動態(tài)特征物件檢測與追蹤的流程圖，圖8是對動態(tài)特征物件檢測與追蹤的應(yīng)用情境。在此，假設(shè)載體是不動的，而在環(huán)境內(nèi)(比如室內(nèi))有多個移動的特征物件810A 810C。 如圖7所示，在步驟710中，根據(jù)第一傳感器信息來預(yù)測動態(tài)特征物件的移動量。在此，可利用傳感器110a及/或110b來感測至少一個動態(tài)特征物件的移動量。其方式如下。 追蹤動態(tài)特征物件的運動模型如下
<formula>formula see original document page 14</formula>
其中Ot =[《,。
<formula>formula see original document page 14</formula>
速度，而v,k,,《，，<
征物件的移動
<formula>formula see original document page 14</formula>
<formula>formula see original document page 14</formula>、V]T為第一個動態(tài)特征物件在空間中的位置與速度，
、/]T為第N個動態(tài)特征物件(N為正整數(shù))在空間中的位置與_:為物體在空間中的加速度，e T,t為動態(tài)特N， n為正整數(shù))運動模型矩陣如下
<formula>formula see original document page 14</formula>
經(jīng)由此運動模型可估測出動態(tài)特征物件在空間中的位置。需注意的是在此把動態(tài)特征物件的移動視為加速度為常數(shù)且有誤差的信息，依可粗略估出物件的移動位置。此外，還可利用感測模型以修正動態(tài)特征物件的真正位置。 接著，如步驟720所示，提取第二傳感器信息，其亦用于測量環(huán)境特征物件，比如感測其移動量。接著，如步驟730所示，提取第三傳感器信息，其亦用于測量環(huán)境特征物件，比如感測其移動量。 接著，如步驟740所示，比較目前所接收到的第二 第三傳感器信息，以決定被感測的動態(tài)特征物件是否為已知的。如果是，則根據(jù)目前所接收到的第二至第三傳感器信息來修正環(huán)境特征物件的狀態(tài)與位置，并檢測與追蹤，如步驟750所示。如果在步驟740的決定為否，代表被感測的動態(tài)特征物件乃是新的動態(tài)特征物件，如此，則加入新的動態(tài)特征物件的位置與其狀態(tài)于地圖中，并檢測與追蹤，如步驟760所示。 在步驟740中，進行比較的方法有兩種，一種為異質(zhì)比對(homogeneous)，一種為
同質(zhì)比對(non-homogeneous)。異質(zhì)比對方式為，當物件僅具有一種特性時，利用電磁波傳
感器與紅外線熱傳感器比較出傳感器信息的差異性，進而追蹤僅具有一種特性的物件。同
質(zhì)比對方式則為，當物件具有兩種特性時，利用視覺傳感器與超音波傳感器比較傳感器信
息間的異同，進而追蹤此物件。 在圖7中所用的感測模型如下 Zt = T(Xt)+S Tt 其中S^為傳感器的噪聲。 若傳感器為視覺傳感器或其它可量得物體在空間中位置的傳感器，則感測模型如下
<formula>formula see original document page 14</formula>0 0 1
0 1 0
1 0 0 若傳感器為超音波傳感器或電磁波傳感器或其它距離信息(range-only)傳感 器，則感測模型如下 、 =r、.(z,) + ~.、、， 此外，在步驟750與760中，感測模型可估出物件在空間中的位置，而由運動模型 所估測出的物件位置可經(jīng)由感測模型修正，以得到物件在空間中的較精確位置與較精確速 度，達到檢測與追蹤物體的目的。 此外，本發(fā)明實施例還可結(jié)合圖5的定位與建地圖，及圖7的移動物件檢測與追 蹤，以達到定位、建地圖、移動物件檢測與追蹤，其應(yīng)用情境如圖9所示。在圖9中，假設(shè)手 920令載體120為動態(tài)(只移動不轉(zhuǎn)動、不移動只轉(zhuǎn)動、或又移動又轉(zhuǎn)動)，特征物件910A 910C為靜態(tài)，而特征物件910D為動態(tài)。由上述說明可知，如何對載體120檢測其狀態(tài)(比 如，預(yù)估其姿態(tài)(posture))、如何建地圖、如何檢測與追蹤動態(tài)的特征物件910D，故其細節(jié) 不再重述。在此，若載體120為動態(tài)，則檢測與追蹤的算法是基于移動中的載體，所以需考 慮載體的位置與不確定性、并預(yù)測載體的位置(類似圖5的做法)。 綜上所述可知，在本發(fā)明實施例中，利用可互補的多種傳感器，來精準定位、追蹤、 檢測、預(yù)估載體狀態(tài)(姿態(tài))。故而，本發(fā)明可應(yīng)用于比如但不受限于，飛機的慣性導(dǎo)航系 統(tǒng)、數(shù)字相機的防手震系統(tǒng)、車輛速度檢測系統(tǒng)、車輛避撞系統(tǒng)、電視游樂器(如Wii)的把 手的三維姿態(tài)檢測等、手機定位、室內(nèi)地圖產(chǎn)生器。此外，本發(fā)明更可應(yīng)用于室內(nèi)伴侶機器 人，其可監(jiān)控環(huán)境里的老人、小孩等。本發(fā)明還可應(yīng)用于車輛，其可監(jiān)控環(huán)境里的車輛，避免 車禍。本發(fā)明亦可應(yīng)用于可移動的機器人，其可檢測移動中的人類，進而追蹤此人并提供服 務(wù)。 綜上所述，雖然本發(fā)明已以實施例公開如上，然其并非用以限定本發(fā)明。本發(fā)明所 屬技術(shù)領(lǐng)域中具有通常知識者，在不脫離本發(fā)明的精神和范圍內(nèi)，當可作各種的更動與潤 飾。因此，本發(fā)明的保護范圍當視所附的權(quán)利要求所界定的為準。
權(quán)利要求
一種感測系統(tǒng)，其特征在于，該系統(tǒng)包括一載體；一多重傳感器模塊，配置于該載體上，該多重傳感器模塊感測多種彼此互補的特性，該多重傳感器模塊感測該載體以得到一載體信息，該多重傳感器模塊還感測一特征物件以得到一特征物件信息；一控制器，接收該多重傳感器模塊所傳來的該載體信息與該特征物件信息；以及一顯示單元，受控于該控制器，以提供一反應(yīng)信號；其中，該控制器執(zhí)行下列的至少一者該控制器將該載體定位于一地圖內(nèi)，且該控制器還將該特征物件加入于該地圖中及更新在該地圖中的該特征物件；以及根據(jù)該特征物件信息，該控制器預(yù)測該特征物件的一移動量，以決定該特征物件是否為已知的，并據(jù)以修正該地圖及將該特征物件加入于該地圖中。
2. 如權(quán)利要求1所述的系統(tǒng)，其特征在于，該多重傳感器模塊包括可見光視覺傳感 器、不可見光視覺傳感器、電磁波傳感器、紅外線熱感應(yīng)器與紅外線距離傳感器的至少一者 或其組合。
3. 如權(quán)利要求1所述的系統(tǒng)，其特征在于，該多重傳感器模塊包括超音波感應(yīng)器、超 音波數(shù)組感應(yīng)器與聲納感應(yīng)器的至少一者或其組合。
4. 如權(quán)利要求1所述的系統(tǒng)，其特征在于，該多重傳感器模塊包括加速規(guī)、陀螺儀與 轉(zhuǎn)速計數(shù)組的至少一者或其組合。
5. 如權(quán)利要求l所述的系統(tǒng)，其特征在于，該顯示單元所提供的該反應(yīng)信號包括一語 音信號、一圖像信號與一提示信號的至少一者或其組合。
6. 如權(quán)利要求1所述的系統(tǒng)，其特征在于，該載體包括車輛、機車、自行車、機器人、眼 鏡、手表、安全帽或其它可移動物體的至少一者或其組合。
7. 如權(quán)利要求1所述的系統(tǒng)，其特征在于，該控制器 根據(jù)該載體信息，預(yù)測該載體的一狀態(tài)；比較被視為靜態(tài)的該特征物件的該特征物件信息與該地圖，以決定該特征物件是否在 該地圖內(nèi)；如果該特征物件未顯示在該地圖內(nèi)，則將該特征物件的一狀態(tài)與一位置加入于該地圖 中；以及如果該特征物件顯示在該地圖內(nèi)，則修正該地圖、修正該載體的一位置、修正該載體的 該狀態(tài)。
8. 如權(quán)利要求1所述的系統(tǒng)，其特征在于，該控制器比較被視為動態(tài)的該特征物件的該特征物件信息與該地圖，以決定該特征物件是否為 已知的；如果該特征物件為已知的，則修正該特征物件在該地圖中的一位置與一狀態(tài)；以及 如果該特征物件不是已知的，則將該特征物件的該位置與該狀態(tài)加入于該地圖中。
9. 一種載體定位與建地圖的感測方法，其特征在于，該方法包括 執(zhí)行一第一感測步驟，以感測一載體以得到一載體信息；執(zhí)行一第二感測步驟，以感測一特征物件以得到一特征物件信息，其中該第二感測步驟感測多種彼此互補的特性；分析該載體信息，以得到該載體的一位置與一狀態(tài)，并將該載體定位于一地圖內(nèi)； 分析該特征物件信息，以得到該特征物件的一位置與一狀態(tài)；以及比較一地圖與該特征物件的該位置與該狀態(tài)，以將該特征物件的該位置與該狀態(tài)加入 于該地圖中及更新在該地圖中的該特征物件的該位置與該狀態(tài)。
10. 如權(quán)利要求9所述的方法，其特征在于，該第一感測步驟包括 感測該載體，以得到該載體的一速度、一加速度、一角速度、一角加速度的至少一者。
11. 如權(quán)利要求10所述的方法，其特征在于，該第二感測步驟包括 感測該特征物件，以得到該特征物件與該載體間的一相對距離關(guān)系。
12. 如權(quán)利要求IO所述的方法，其特征在于，還包括 比較該載體的該位置與該特征物件的該位置，以產(chǎn)生一情境反應(yīng)。
13. —種動態(tài)物件檢測與追蹤的感測方法，其特征在于，該方法包括 執(zhí)行一第一感測步驟，以感測一動態(tài)物件以得到其第一移動量；執(zhí)行一第二感測步驟，以感測該動態(tài)物件以得到其第二移動量，其中該第一感測步驟 與該第二感測步驟彼此互補；分析該第一移動量與該第二移動量，以預(yù)估一載體與該動態(tài)物件間的一相對距離； 決定該動態(tài)物件是否為已知的；若為已知，修正在一地圖中的該動態(tài)物件的一狀態(tài)，并檢測與追蹤；以及 若為未知，將該動態(tài)物件及其狀態(tài)加入于該地圖，并檢測與追蹤。
14. 如權(quán)利要求13所述的方法，其特征在于，還包括 分析該載體與該動態(tài)物件間的該相對距離，以產(chǎn)生一情境反應(yīng)。
15. 如權(quán)利要求13所述的方法，其特征在于，如果該載體為動態(tài)，該方法還包括 感測該載體，以得到該載體的一位置、一速度、一加速度、一角速度、一角加速度的至少一者。
全文摘要
本發(fā)明涉及一種利用感測元件的定位與檢測的系統(tǒng)與方法，利用彼此互補的多種傳感器，進而達到定位與建地圖。此外，在動態(tài)物件檢測與追蹤時，將多重傳感器對物體的感測結(jié)果進行交叉比對，以檢測出移動中的物體的位置并追蹤此物體。
文檔編號G01C21/00GK101750060SQ20081018608
公開日2010年6月23日 申請日期2008年12月22日 優(yōu)先權(quán)日2008年12月22日
發(fā)明者唐之瑋, 曾國師, 李金龍, 楊安陶, 郭家霖 申請人:財團法人工業(yè)技術(shù)研究院