專利名稱：定位載體、估測載體姿態(tài)與建地圖的系統(tǒng)與方法
技術(shù)領(lǐng)域：
本發(fā)明是有關(guān)于一種應(yīng)用感測元件以定位載體、估測載體姿態(tài)與建地圖的系統(tǒng)與 方法，且特別是有關(guān)于一種應(yīng)用可互補(bǔ)的多種感測元件的系統(tǒng)與方法，其能定位載體、估測 環(huán)境特征點(diǎn)的位置并建構(gòu)地圖。
背景技術(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)境(比如室內(nèi)的 對象、人物等)的變動性大。目前來說，室內(nèi)定位的技術(shù)有兩種類型，一種為外部定位系統(tǒng)，另一種為內(nèi)部定位 系統(tǒng)。外部定位系統(tǒng)例如，利用外部的傳感器與機(jī)器人的接收器之間的相對關(guān)系去估測機(jī) 器人在空間中的位置。由內(nèi)部定位系統(tǒng)例如，在機(jī)器人上放傳感器，將所掃描到的數(shù)據(jù)比較 于其內(nèi)建地圖，進(jìn)而估測機(jī)器人在空間中的位置。外部定位系統(tǒng)的定位速度快，但需事先建好外部傳感器。一旦外部傳感器被移動 或被遮蔽，此系統(tǒng)將無法定位。外部定位系統(tǒng)如要用于大范圍，所需的傳感器數(shù)目與成本將 增加。內(nèi)部定位系統(tǒng)的定位速度較慢但具擴(kuò)充性，即使環(huán)境變動性大，只要特征點(diǎn)可供 定位，內(nèi)部定位系統(tǒng)仍能定位。但其必需先內(nèi)建室內(nèi)環(huán)境地圖，才能進(jìn)行定位。
發(fā)明內(nèi)容
本發(fā)明實(shí)施例提出一種結(jié)合慣性感測元件(inertial measurement unit，IMU)與 視覺感測元件，以定位移動載體并計算出環(huán)境特征對象在三度空間的相對位置(相對于移 動載體)，達(dá)到同步定位、姿態(tài)估測、與建構(gòu)環(huán)境地圖。本發(fā)明的一實(shí)施例提出一種定位載體、估測載體姿態(tài)與建地圖的系統(tǒng)，包括一慣 性感測元件，測量該載體的一移動狀態(tài)與一旋轉(zhuǎn)狀態(tài)；一視覺感測元件，配置在該載體的一 表面上以拍攝該載體所在的一室內(nèi)環(huán)境內(nèi)的至少一環(huán)境特征點(diǎn)；以及一控制器，控制該慣 性感測元件與該視覺感測元件，接收該慣性感測元件的一感測結(jié)果與該視覺感測元件的一 感測結(jié)果以估測該載體的一姿態(tài)信息、該載體的一位置信息與一速度信息，并建構(gòu)含有該 環(huán)境特征點(diǎn)的一地圖。該控制器根據(jù)該慣性感測元件與該視覺感測元件之一的一修正后感 測結(jié)果而估測，該控制器令該慣性感測元件與該視覺感測元件中的另一個感測并且據(jù)以修 正該載體的該姿態(tài)信息、該載體的該位置信息、該速度信息與該地圖。本發(fā)明的另一實(shí)施例提出一種定位載體、估測載體姿態(tài)與建地圖的方法，包括利 用一慣性感測元件以測量該載體的一移動狀態(tài)與一旋轉(zhuǎn)狀態(tài)；利用一視覺感測元件以拍攝 該載體所在的一室內(nèi)環(huán)境內(nèi)的至少一環(huán)境特征點(diǎn)；以及根據(jù)該慣性感測元件與該視覺感測 元件之一的一修正后感測結(jié)果而估測，令該慣性感測元件與該視覺感測元件中的另一個感測并且據(jù)以修正該載 體的一姿態(tài)信息、該載體的一位置信息、一速度信息與一地圖。為讓本發(fā)明的上述內(nèi)容能更明顯易懂，下文特舉實(shí)施例，并配合所附圖式，作詳細(xì) 說明如下
圖1顯示根據(jù)本發(fā)明第一實(shí)施例的系統(tǒng)示意圖。圖2顯示本第一實(shí)施例如何結(jié)合慣性感測元件與視覺感測元件來估測載體的狀 態(tài)。圖3顯示根據(jù)本發(fā)明第一實(shí)施例的流程圖。附圖標(biāo)記說明100:載體110 慣性感測元件120:視覺感測元件 130:控制器140:顯示單元210 230 環(huán)境特征點(diǎn)241 243 載體的姿態(tài)與位置251 實(shí)際畫面210A 230A 環(huán)境特征點(diǎn)的實(shí)際位置252 估測畫面210B 230B 環(huán)境特征點(diǎn)的估測位置253 實(shí)際畫面210C 230C 環(huán)境特征點(diǎn)的實(shí)際位置El E3 估測誤差310 370 步驟
具體實(shí)施例方式第一實(shí)施例慣性感測元件+視覺感測元件(單眼相機(jī))圖1顯示根據(jù)本發(fā)明第一實(shí)施例的系統(tǒng)示意圖。如圖1所示，根據(jù)本發(fā)明第一實(shí) 施例的系統(tǒng)包括載體100、慣性感測元件110、視覺感測元件120、控制器130與顯示單元 140。載體100是可以移動的，其比如但不受限為可進(jìn)行室內(nèi)定位的機(jī)器人、個人攜帶 型導(dǎo)航裝置(Personal Navigation System)、游戲娛樂機(jī)、車輛、機(jī)車、自行車、眼鏡、手表、 安全帽或其它可移動物體。慣性感測元件110可為下列之一或其任意組合加速度計(accelerometer)、陀螺 儀(gyroscope)、轉(zhuǎn)速計數(shù)組或其它可量測物體力學(xué)參數(shù)的傳感器。加速度計可測量載體 的移動狀態(tài)(如加速度信息)，而陀螺儀則可測量載體的旋轉(zhuǎn)狀態(tài)(如角速度信息)。根據(jù) 慣性感測元件110的感測結(jié)果，控制器130可以估測出移動載體的載體姿態(tài)、速度、位置等。 控制器130可將慣性感測元件110所感測出的加速度、角速度等參數(shù)進(jìn)行積分，以計算出載 體100的速度信息、姿態(tài)信息與位置信息。然而，經(jīng)過長時間積分后，由慣性感測元件110 所計算出的速度信息與位置信息的累積誤差(如速度誤差、位移誤差、姿態(tài)誤差等)無法單 靠慣性感測元件110的感測結(jié)果來消除。此外，慣性感測元件110不會因?yàn)榄h(huán)境的光線昏暗或無光源而影響測量結(jié)果的準(zhǔn)確性。慣性感測元件110的感測速度快。在控制器130的控制下，慣性感測元件110進(jìn)行感測并將其回傳給控制器。在第一實(shí)施例中，視覺感測元件120為單眼相機(jī)，且配置在載體100的表面上，以 拍攝室內(nèi)的環(huán)境特征點(diǎn)。根據(jù)視覺感測元件120的感測結(jié)果，控制器130可測量出環(huán)境特 征點(diǎn)的移動，估測載體的移動/旋轉(zhuǎn)狀態(tài)。視覺感測元件易受光線昏暗或無光源的干擾，而 且，在環(huán)境特征點(diǎn)的成像情況不明顯下(例如，位于白墻前面的白色物體)，單靠視覺感測 元件的感測結(jié)果，控制器不易/無法定位出載體與環(huán)境特征的相對位置。視覺感測元件的 感測操作較慢且控制器130的相對應(yīng)運(yùn)算較復(fù)雜。如果沒有慣性感測元件的感測結(jié)果的輔 助，則單靠視覺感測元件的感測結(jié)果，控制器會花耗大量計算于計算出載體的3D狀態(tài)。如 果載體處在高動態(tài)狀態(tài)，則此計算容易發(fā)散，使得系統(tǒng)可能會出錯。故而，如果有慣性感測 元件的輔助，控制器在計算載體的3D狀態(tài)之時，不會只靠視覺感測元件的感測結(jié)果，所以 控制器的計算量減少；即使載體處在高動態(tài)狀態(tài)，計算仍不易發(fā)散，系統(tǒng)出錯的可能性大為 降低。故而，在本實(shí)施例中，結(jié)合慣性感測元件與視覺感測元件，以計算出機(jī)率模型的感 測融合，來定位出載體在三度空間的位置，及環(huán)境特征點(diǎn)在空間中的相對位置(相對于載 體)，達(dá)到空間定位、姿態(tài)估測、建置地圖的目的。在此，地圖是由環(huán)境特征點(diǎn)所組成，而且， 在本實(shí)施例中，可在載體移動/旋轉(zhuǎn)的過程中，實(shí)時建構(gòu)出地圖。控制器130連接至慣性感測元件110、視覺感測元件120與顯示單元。雖然在圖1 中，控制器130似乎位于載體100之外，但在其它實(shí)施例中，控制器130也可位于載體內(nèi)部。 控制器會控制慣性感測元件110與視覺感測元件120的操作，接收這些感測元件的感測結(jié) 果并進(jìn)行計算，以估測載體姿態(tài)與位置，并建構(gòu)含有環(huán)境特征點(diǎn)的地圖。顯示單元140連接在控制器130。在控制器130的控制下，顯示單元140產(chǎn)生可與 外界互動的反應(yīng)。比如，顯示單元140可提醒使用者的反應(yīng)為影音表達(dá)語音、音樂、預(yù)錄聲 音。此外，顯示單元也可顯示圖像、文字、顏色、明暗、閃爍、圖形等。顯示單元140所提供 的影音表達(dá)語音包括語音信號、影像信號與提示信號的至少一者或其組合?，F(xiàn)請參考圖2，其顯示本第一實(shí)施例如何結(jié)合慣性感測元件與視覺感測元件 來估測載體的狀態(tài)。圖2所示為視覺同時定位與建地圖(VSLAM，VisionSimultaneous Localization and Mapping)。在圖2中，210 230代表在環(huán)境中的三個環(huán)境特征點(diǎn)。請 注意，雖然在此以3個環(huán)境特征點(diǎn)為例，但本實(shí)施例并不受限于此。在時刻t-Ι時，載體的姿態(tài)與位置(t-111-1)如參考附圖標(biāo)記241所示；而且，時 亥Ijt-I時，視覺感測元件會拍攝實(shí)際畫面251，其中，實(shí)際畫面251顯示出環(huán)境特征點(diǎn)的實(shí)際 位置210A 230A。因?yàn)檩d體的移動/旋轉(zhuǎn)，在時刻t時，根據(jù)慣性感測元件的感測結(jié)果，控制器估測 載體的未修正姿態(tài)與位置(tlt-Ι)如參考附圖標(biāo)記242所示。根據(jù)載體的未修正姿態(tài)與位置(t 11-1) 242，控制器估測出畫面252，其包括對環(huán) 境特征點(diǎn)的估測位置210B 230B。在時刻t時，在控制器的控制下，視覺感測元件拍攝實(shí)際畫面253并回傳給控制 器，實(shí)際畫面253包括環(huán)境特征點(diǎn)的實(shí)際位置210C 230C?？刂破鲗Ρ拳h(huán)境特征點(diǎn)的估測 位置210B 230B與實(shí)際位置210C 230C，以計算出估測誤差El E3，此誤差可能包括控制器在量測環(huán)境特征點(diǎn)時的誤差及控制器估測載體的姿態(tài)與位置的誤差。在本實(shí)施例中，控制器可逐一修正所有環(huán)境特征點(diǎn)的位置誤差，或是同時修正所 有環(huán)境特征點(diǎn)的位置誤差。在本實(shí)施例中，修正誤差的方式可有多種，比如可根據(jù)誤差 El E3的平均值進(jìn)行修正。根據(jù)誤差El E3，控制器可以推算出對載體的姿態(tài)與位置的修正量，并據(jù)以修正 載體的姿態(tài)與位置的狀態(tài)，如參考附圖標(biāo)記243所示。當(dāng)控制器欲命令慣性感測元件進(jìn)行 量測時，控制器會先根據(jù)修正后姿態(tài)與位置信息(t |t) 243來進(jìn)行估測，如此可以修正因慣 性感測元件的積分結(jié)果所導(dǎo)致的累積誤差。如上述，在本實(shí)施例中，當(dāng)要估測載體的姿態(tài)、位置與建構(gòu)地圖時，要進(jìn)行估測 (由控制器進(jìn)行)_量測(由其中一種感測元件進(jìn)行)_修正(由控制器進(jìn)行)。當(dāng)控制器 在進(jìn)行估測時，根據(jù)控制器對另一種感測元件的修正后感測結(jié)果而進(jìn)行估測，之后，控制器 會令其中一種感測元件進(jìn)行數(shù)據(jù)感測并且依據(jù)其中一種感測元件的感測數(shù)據(jù)來進(jìn)行修正?，F(xiàn)請參考圖3，其顯示根據(jù)本發(fā)明第一實(shí)施例的流程圖。如步驟310所示，在命令 慣性感測元件進(jìn)行量測之前，控制器會先估測載體的3D姿態(tài)、速度、位置。接著，如步驟320所示，在控制器的控制下，慣性感測元件測量載體的移動狀態(tài) (如加速度信息)與旋轉(zhuǎn)狀態(tài)(如角速度信息)，并將感測結(jié)果回傳給控制器。之后，如步驟330所示，根據(jù)慣性感測元件的感測結(jié)果，控制器會修正載體的3D姿 態(tài)、速度、位置等。另一方面，如步驟340所示，在命令視覺感測元件進(jìn)行量測之前，控制器會先進(jìn)行 估測(如估測載體的3D姿態(tài)、速度、位置)。本實(shí)施例中，當(dāng)控制器已得到慣性感測元件的 修正后結(jié)果，則控制器會依據(jù)慣性感測元件的修正后結(jié)果來進(jìn)行步驟340中的估測。接著，如步驟350所示，在控制器的控制下，視覺感測元件進(jìn)行測量(即，視覺感測 元件會拍攝畫面)，并將感測結(jié)果回傳給控制器。之后，如步驟360所示，根據(jù)視覺感測元件的感測結(jié)果，控制器會修正載體的3D姿 態(tài)、速度、位置等。詳細(xì)地說，如步驟361所示，根據(jù)視覺感測元件所拍攝的實(shí)際畫面，控制 器比較環(huán)境特征點(diǎn)的估測位置與實(shí)際位置間的差異，以計算出誤差。接著，如步驟362所示，依據(jù)所算出的誤差，控制器修正對所有環(huán)境特征點(diǎn)的估測 位置。在步驟362中，控制器可逐一修正所有環(huán)境特征點(diǎn)的位置誤差，或是同時修正所有環(huán) 境特征點(diǎn)的位置誤差。如步驟363所示，根據(jù)上述步驟362所計算出的誤差，控制器修正對載體預(yù)測位 置，比如，修正所估測的載體位置、載體姿態(tài)等信息。如步驟370所示，經(jīng)由估測_測量_修正結(jié)果，可以對載體定位，估測載體的3D姿 態(tài)，并建構(gòu)環(huán)境地圖(含有環(huán)境特征點(diǎn))。如果在控制器欲進(jìn)行步驟310的估測時，控制器已得到視覺感測元件的修正后結(jié) 果(即步驟370已完成)，則控制器會依據(jù)視覺感測元件的修正后結(jié)果來進(jìn)行步驟310的 估測。相似地，如果在控制器欲進(jìn)行步驟340的估測時，控制器已得到慣性感測元件的修正 后結(jié)果(即步驟330已完成)，則控制器會依據(jù)慣覺感測元件的修正后結(jié)果來進(jìn)行步驟340 的估測。在本實(shí)施例中，如果慣性感測元件與視覺感測元件的操作速度不同步的話(通常，慣性感測元件的操作速度較快而視覺感測元件的操作速度較慢)，則在圖3中，步驟
310 330的操作不同于步驟340 370。
底下將詳述本實(shí)施例的詳細(xì)做法。利用慣性感測元件來定位的狀態(tài)定義如下
Xt =g (xt-i，Ut)+ ￡t
Zt =h(xt)+ δ,
假設(shè)運(yùn)動模型(Motion Model)為 Xt = g(Xt_1 Ut)+ε t.，則載體狀態(tài)為
Xt =[xG,tVx, tAx，t YG, t Vy, t Ay，t ZG，t Vz，t A; T ；，t eO, t eI, t e2, t e3，tJ
其中9
[xG,tYg , tZG,Jt為載體在世界坐標(biāo)中的絕對位置，
[vx,tvy, tVz,Jt為載體在載體坐標(biāo)中的速度，
[Ax,tK,tAz,Jt為載體在載體坐標(biāo)中的加速度，
[e0,tei.te2,t e3,t]T為載體在載體坐標(biāo)中的四元II (quaternion)，
Ut =[ax,tay,taz,t x,t coy,t ωζ,t]T為載體在載體坐標(biāo)中的加速度與角速度。 要算出載體在t時在世界坐標(biāo)中的絕對位置Bt，需要載體在t-Ι時在世界坐標(biāo)中 的絕對位置、載體上的加速規(guī)與陀螺儀所得到加速度和角速度的積分信息，且利用四元數(shù) 把載體坐標(biāo)信息經(jīng)由載體坐標(biāo)轉(zhuǎn)換成世界坐標(biāo)，而且，以上過程在運(yùn)動模型中完成。
載體狀態(tài)的運(yùn)動模型表示如下
權(quán)利要求
1.一種定位載體、估測載體姿態(tài)與建地圖的系統(tǒng)，包括 一慣性感測元件，測量該載體的一移動狀態(tài)與一旋轉(zhuǎn)狀態(tài)；一視覺感測元件，配置在該載體的一表面上以拍攝該載體所在的一室內(nèi)環(huán)境內(nèi)的至少 一環(huán)境特征點(diǎn)；以及一控制器，控制該慣性感測元件與該視覺感測元件，接收該慣性感測元件的一感測結(jié) 果與該視覺感測元件的一感測結(jié)果以估測該載體的一姿態(tài)信息、該載體的一位置信息與一 速度信息，并建構(gòu)含有該環(huán)境特征點(diǎn)的一地圖； 其中，該控制器根據(jù)該慣性感測元件與該視覺感測元件之一的一修正后感測結(jié)果而估測，之 后，該控制器令該慣性感測元件與該視覺感測元件中的另一個感測并且據(jù)以修正該載體的 該姿態(tài)信息、該載體的該位置信息、該速度信息與該地圖。
2.如申請專利范圍第1項(xiàng)所述的系統(tǒng)，其中，該控制器在命令該慣性感測元件進(jìn)行量測之前，估測該載體的該姿態(tài)信息、該位置信 息與該速度信息；該控制器如果在該控制器估測之前，已計算出該視覺感測元件的該修正后感測結(jié)果， 則該控制器依據(jù)該視覺感測元件的該修正后感測結(jié)果進(jìn)行估測；該慣性感測元件在該控制器的控制下，測量該載體的移動狀態(tài)與旋轉(zhuǎn)狀態(tài)，并將該感 測結(jié)果回傳給該控制器；以及該控制器根據(jù)該慣性感測元件的該感測結(jié)果，修正該姿態(tài)信息、該位置信息與該速度 fn息ο
3.如申請專利范圍第2項(xiàng)所述的系統(tǒng)，其中，該移動狀態(tài)至少包括一加速度信息，與該 旋轉(zhuǎn)狀態(tài)至少包括一角速度信息。
4.如申請專利范圍第3項(xiàng)所述的系統(tǒng)，其中，該控制器在命令該視覺感測元件進(jìn)行測量之前，先估測該載體的該姿態(tài)信息、該位置 信息與該速度信息；該控制器如果在該控制器估測之前，已計算出該慣性感測元件的該修正后感測結(jié)果， 則該控制器依據(jù)該慣性感測元件的該修正后結(jié)果進(jìn)行估測；該視覺感測元件在該控制器的控制下，拍攝該載體所在室內(nèi)的該環(huán)境特征點(diǎn)畫面，并 將該感測結(jié)果回傳給該控制器；以及該控制器根據(jù)該視覺感測元件的該感測結(jié)果，修正該姿態(tài)信息、該位置信息與該速度 fn息ο
5.如申請專利范圍第4項(xiàng)所述的系統(tǒng)，其中，該控制器根據(jù)該視覺感測元件所拍攝的一實(shí)際畫面，比較該環(huán)境特征點(diǎn)的一估測位置 與一實(shí)際位置間的差異，以計算出一誤差；該控制器依據(jù)所算出的該誤差，修正對該環(huán)境特征點(diǎn)的該估測位置，并更新該地圖；以及該控制器根據(jù)該誤差，修正該姿態(tài)信息、該位置信息與該速度信息。
6.如申請專利范圍第5項(xiàng)所述的系統(tǒng)，其中，如果該室內(nèi)環(huán)境具有復(fù)數(shù)環(huán)境特征點(diǎn)，則 該控制器逐一修正該些環(huán)境特征點(diǎn)的各別估測位置，或是該控制器同時修正該些環(huán)境特征點(diǎn)的各別估測位置。
7.如申請專利范圍第1項(xiàng)所述的系統(tǒng)，其中，該視覺感測元件包括下列的至少一者或 其任意組合一個或復(fù)數(shù)個單眼相機(jī)，一個或復(fù)數(shù)個全像鏡頭相機(jī)。
8.如申請專利范圍第1項(xiàng)所述的系統(tǒng)，其中，該慣性感測元件包括可為下列的至少一 者或其任意組合一加速規(guī)、一陀螺儀、一轉(zhuǎn)速計數(shù)組。
9.如申請專利范圍第1項(xiàng)所述的系統(tǒng)，更包括一顯示單元，連接在該控制器，在該控制器的控制下，該顯示單元產(chǎn)生一外界互動反應(yīng)。
10.如申請專利范圍第9項(xiàng)所述的系統(tǒng)，其中，該顯示單元顯示一影音表達(dá)語音、一音 樂、一預(yù)錄聲音。
11.一種定位載體、估測載體姿態(tài)與建地圖的方法，包括利用一慣性感測元件以測量該載體的一移動狀態(tài)與一旋轉(zhuǎn)狀態(tài)； 利用一視覺感測元件以拍攝該載體所在的一室內(nèi)環(huán)境內(nèi)的至少一環(huán)境特征點(diǎn)；以及 根據(jù)該慣性感測元件與該視覺感測元件之一的一修正后感測結(jié)果而估測，令該慣性感 測元件與該視覺感測元件中的另一個感測并且據(jù)以修正該載體的一姿態(tài)信息、該載體的一 位置信息、一速度信息與一地圖。
12.如申請專利范圍第11項(xiàng)所述的方法，更包括在該慣性感測元件量測之前，估測該載體的該姿態(tài)信息、該位置信息與該速度信息； 如果在該估測步驟之前，已計算出該視覺感測元件的該修正后感測結(jié)果，則依據(jù)該視 覺感測元件的該修正后感測結(jié)果進(jìn)行估測；該慣性感測元件測量該載體的移動狀態(tài)與旋轉(zhuǎn)狀態(tài)，并回傳該感測結(jié)果；以及 根據(jù)該慣性感測元件的該感測結(jié)果，修正該姿態(tài)信息、該位置信息與該速度信息。
13.如申請專利范圍第12項(xiàng)所述的方法，其中，在該視覺感測元件測量之前，先估測該載體的該姿態(tài)信息、該位置信息與該速度信息；如果在該估測步驟之前，已計算出該慣性感測元件的該修正后感測結(jié)果，則依據(jù)該慣 性感測元件的該修正后結(jié)果進(jìn)行估測；該視覺感測元件拍攝該載體所在室內(nèi)的該環(huán)境特征點(diǎn)畫面，并回傳該感測結(jié)果；以及 根據(jù)該視覺感測元件的該感測結(jié)果，修正該姿態(tài)信息、該位置信息與該速度信息。
14.如申請專利范圍第13項(xiàng)所述的方法，其中，根據(jù)該視覺感測元件所拍攝的一實(shí)際畫面，比較該環(huán)境特征點(diǎn)的一估測位置與一實(shí)際 位置間的差異，以計算出一誤差；依據(jù)所算出的該誤差，修正對該環(huán)境特征點(diǎn)的該估測位置，并更新該地圖；以及 根據(jù)該誤差，修正該姿態(tài)信息、該位置信息與該速度信息。
15.如申請專利范圍第14項(xiàng)所述的方法，其中，如果該室內(nèi)環(huán)境具有復(fù)數(shù)環(huán)境特征點(diǎn)， 則逐一修正該些環(huán)境特征點(diǎn)的各別估測位置，或是同時修正該些環(huán)境特征點(diǎn)的各別估測位置。
全文摘要
一種定位載體、估測載體姿態(tài)與建地圖的系統(tǒng)，包括一慣性感測元件，測量該載體的一移動狀態(tài)與一旋轉(zhuǎn)狀態(tài)；一視覺感測元件，配置在該載體的一表面上以拍攝該載體所在的一室內(nèi)環(huán)境內(nèi)的至少一環(huán)境特征點(diǎn)；以及一控制器，控制該慣性感測元件與該視覺感測元件，接收該慣性感測元件的一感測結(jié)果與該視覺感測元件的一感測結(jié)果以估測該載體的一姿態(tài)信息、該載體的一位置信息與一速度信息，并建構(gòu)含有該環(huán)境特征點(diǎn)的一地圖。該控制器根據(jù)該慣性感測元件與該視覺感測元件之一的一修正后感測結(jié)果而估測，該控制器令該慣性感測元件與該視覺感測元件中的另一個感測并且據(jù)以修正該載體的該姿態(tài)信息、該載體的該位置信息、該速度信息與該地圖。
文檔編號G01C21/20GK102109348SQ20091026551
公開日2011年6月29日 申請日期2009年12月25日 優(yōu)先權(quán)日2009年12月25日
發(fā)明者張彥中, 李金龍, 游鴻修 申請人:財團(tuán)法人工業(yè)技術(shù)研究院