TWI397671B - 定位載體、估測載體姿態與建地圖之系統與方法 - Google Patents

Description

定位載體、估測載體姿態與建地圖之系統與方法

本發明是有關於一種應用感測元件以定位載體、估測載體姿態與建地圖之系統與方法，且特別是有關於一種應用可互補的多種感測元件之系統與方法，其能定位載體、估測環境特徵點的位置並建構地圖。

室外定位系統，比如GPS(全球定位系統)，已廣泛使用於車用導航系統，以在室外定出車輛或人的位置。但室內定位系統至今仍有其問題無法突破，室內定位系統之因難度在於(1)在室內時，電磁訊號易被遮蔽，無法接收到衛星訊號；(2)室內環境(比如室內的物件、人物等)的變動性大。

目前來說，室內定位的技術有兩種類型，一種為外部定位系統，另一種為內部定位系統。外部定位系統例如，利用外部的感測器與機器人的接收器之間的相對關係去估測機器人在空間中的位置。由內部定位系統例如，在機器人上放感測器，將所掃描到的資料比較於其內建地圖，進而估測機器人在空間中的位置。

外部定位系統的定位速度快，但需事先建好外部感測器。一旦外部感測器被移動或被遮蔽，此系統將無法定位。外部定位系統如要用於大範圍，所需的感測器數目與成本將增加。

內部定位系統的定位速度較慢但具擴充性，即使環境變動性大，只要特徵點可供定位，內部定位系統仍能定位。但其必需先內建室內環境地圖，才能進行定位。

本發明實施例提出一種結合慣性感測元件(inertial measurement unit,IMU)與視覺感測元件，以定位移動載體並計算出環境特徵物件在三度空間的相對位置(相對於移動載體)，達到同步定位、姿態估測、與建構環境地圖。

本發明之一實施例提出一種定位載體、估測載體姿態與建地圖之系統，包括：一慣性感測元件，測量該載體的一移動狀態與一旋轉狀態；一視覺感測元件，配置於該載體的一表面上以拍攝該載體所在的一室內環境內的至少一環境特徵點；以及一控制器，控制該慣性感測元件與該視覺感測元件，接收該慣性感測元件之一感測結果與該視覺感測元件之一感測結果以估測該載體之一姿態資訊、該載體之一位置資訊與一速度資訊，並建構含有該環境特徵點的一地圖。該控制器根據該慣性感測元件與該視覺感測元件之一之一修正後感測結果而估測，該控制器令該慣性感測元件與該視覺感測元件之另一感測並且據以修正該載體之該姿態資訊、該載體之該位置資訊、該速度資訊與該地圖。

本發明之另一實施例提出一種定位載體、估測載體姿態與建地圖之方法，包括：利用一慣性感測元件以測量該載體的一移動狀態與一旋轉狀態；利用一視覺感測元件以拍攝該載體所在的一室內環境內的至少一環境特徵點；以及根據該慣性感測元件與該視覺感測元件之一之一修正後感測結果而估測，令該慣性感測元件與該視覺感測元件之另一感測並且據以修正該載體之一姿態資訊、該載體之一位置資訊、一速度資訊與一地圖。。

為讓本發明之上述內容能更明顯易懂，下文特舉實施例，並配合所附圖式，作詳細說明如下：

第一實施例：慣性感測元件+視覺感測元件(單眼相機)

第1圖顯示根據本發明第一實施例之系統示意圖。如第1圖所示，根據本發明第一實施例之系統包括：載體100、慣性感測元件110、視覺感測元件120、控制器130與顯示單元140。

載體100是可以移動的，其比如但不受限為可進行室內定位的機器人、個人攜帶型導航裝置(Personal Navigation System)、遊戲娛樂機、車輛、機車、自行車、眼鏡、手錶、安全帽或其他可移動物體。

慣性感測元件110可為下列之一或其任意組合：加速度計(accelerometer)、陀螺儀(gyroscope)、轉速計陣列或其他可量測物體力學參數的感測器。加速度計可測量載體的移動狀態(如加速度資訊)，而陀螺儀則可測量載體的旋轉狀態(如角速度資訊)。根據慣性感測元件110的感測結果，控制器130可以估測出移動載體的載體姿態、速度、位置等。控制器130可將慣性感測元件110所感測出的加速度、角速度等參數進行積分，以計算出載體100的速度資訊、姿態資訊與位置資訊。然而，經過長時間積分後，由慣性感測元件110所計算出的速度資訊與位置資訊的累積誤差(如速度誤差、位移誤差、姿態誤差等)無法單靠慣性感測元件110的感測結果來消除。此外，慣性感測元件110不會因為環境的光線昏暗或無光源而影響測量結果的準確性。慣性感測元件110的感測速度快。在控制器130的控制下，慣性感測元件110進行感測並將之回傳給控制器。

於第一實施例中，視覺感測元件120為單眼相機，且配置於載體100的表面上，以拍攝室內的環境特徵點。根據視覺感測元件120的感測結果，控制器130可測量出環境特徵點的移動，估測載體的移動/旋轉狀態。視覺感測元件易受光線昏暗或無光源的干擾，而且，在環境特徵點的成像情況不明顯下(例如，位於白牆前面的白色物體)，單靠視覺感測元件的感測結果，控制器不易/無法定位出載體與環境特徵的相對位置。視覺感測元件的感測操作較慢且控制器130之相對應運算較複雜。如果沒有慣性感測元件的感測結果的輔助，則單靠視覺感測元件的感測結果，控制器會花耗大量計算於計算出載體的3D狀態。如果載體處於高動態狀態，則此計算容易發散，使得系統可能會出錯。故而，如果有慣性感測元件的輔助，控制器在計算載體的3D狀態之時，不會只靠視覺感測元件的感測結果，所以控制器之計算量減少；即使載體處於高動態狀態，計算仍不易發散，系統出錯的可能性大為降低。

故而，於本實施例中，結合慣性感測元件與視覺感測元件，以計算出機率模型的感測融合，來定位出載體在三度空間的位置，及環境特徵點在空間中的相對位置(相對於載體)，達到空間定位、姿態估測、建置地圖的目的。在此，地圖乃是由環境特徵點所組成，而且，在本實施例中，可在載體移動/旋轉的過程中，即時建構出地圖。

控制器130連接至慣性感測元件110、視覺感測元件120與顯示單元。雖然於第1圖中，控制器130似乎位於載體100之外，但於其他實施例中，控制器130亦可位於載體內部。控制器會控制慣性感測元件110與視覺感測元件120之操作，接收這些感測元件之感測結果並進行計算，以估測載體姿態與位置，並建構含有環境特徵點的地圖。

顯示單元140連接於控制器130。在控制器130之控制下，顯示單元140產生可與外界互動之反應。比如，顯示單元140可提醒使用者之反應為：影音表達語音、音樂、預錄聲音。此外，顯示單元亦可顯示：圖像、文字、顏色、明暗、閃爍、圖形等。顯示單元140所提供之影音表達語音包括：語音信號、影像信號與提示信號之至少一者或其組合。

現請參考第2圖，其顯示本第一實施例如何結合慣性感測元件與視覺感測元件來估測載體的狀態。第2圖所示為視覺同時定位與建地圖(VSLAM,Vision Simultaneous Localization and Mapping)。於第2圖中，210~230代表在環境中的三個環境特徵點。請注意，雖然在此以3個環境特徵點為例，但本實施例並不受限於此。

於時刻t-1時，載體之姿態與位置(t-1|t-1)如參考符號241所示；而且，時刻t-1時，視覺感測元件會拍攝實際畫面251，其中，實際畫面251顯示出環境特徵點之實際位置210A~230A。

因為載體的移動/旋轉，於時刻t時，根據慣性感測元件的感測結果，控制器估測載體之未修正姿態與位置(t|t-1)如參考符號242所示。

根據載體之未修正姿態與位置(t|t-1)242，控制器估測出畫面252，其包括對環境特徵點之估測位置210B~230B。

於時刻t時，在控制器的控制下，視覺感測元件拍攝實際畫面253並回傳給控制器，實際畫面253包括環境特徵點之實際位置210C~230C。控制器對比環境特徵點的估測位置210B~230B與實際位置210C~230C，以計算出估測誤差E1~E3，此誤差可能包括控制器在量測環境特徵點時的誤差及控制器估測載體之姿態與位置的誤差。

於本實施例中，控制器可逐一修正所有環境特徵點之位置誤差，或是同時修正所有環境特徵點之位置誤差。於本實施例中，修正誤差之方式可有多種，比如可根據誤差E1~E3之平均值進行修正。

根據誤差E1~E3，控制器可以推算出對載體的姿態與位置的修正量，並據以修正載體的姿態與位置的狀態，如參考符號243所示。當控制器欲命令慣性感測元件進行量測時，控制器會先根據修正後姿態與位置資訊(t|t)243來進行估測，如此可以修正因慣性感測元件之積分結果所導致的累積誤差。

如上述，於本實施例中，當要估測載體之姿態、位置與建構地圖時，要進行估測(由控制器進行)-量測(由其中一種感測元件進行)-修正(由控制器進行)。當控制器在進行估測時，其乃是根據控制器對另一種感測元件之修正後感測結果而進行估測，之後，控制器會令其中一種感測元件進行資料感測並且依據其中一種感測元件之感測資料來進行修正。

現請參考第3圖，其顯示根據本發明第一實施例之流程圖。如步驟310所示，在命令慣性感測元件進行量測之前，控制器會先估測載體的3D姿態、速度、位置。

接著，如步驟320所示，在控制器之控制下，慣性感測元件測量載體的移動狀態(如加速度資訊)與旋轉狀態(如角速度資訊)，並將感測結果回傳給控制器。

之後，如步驟330所示，根據慣性感測元件之感測結果，控制器會修正載體的3D姿態、速度、位置等。

另一方面，如步驟340所示，在命令視覺感測元件進行量測之前，控制器會先進行估測(如估測載體的3D姿態、速度、位置)。此本實施例中，當控制器已得到慣性感測元件之修正後結果，則控制器會依據慣性感測元件之修正後結果來進行步驟340中之估測。

接著，如步驟350所示，在控制器之控制下，視覺感測元件進行測量(亦即，視覺感測元件會拍攝畫面)，並將感測結果回傳給控制器。

之後，如步驟360所示，根據視覺感測元件之感測結果，控制器會修正載體的3D姿態、速度、位置等。詳細地說，如步驟361所示，根據視覺感測元件所拍攝之實際畫面，控制器比較環境特徵點之估測位置與實際位置間之差異，以計算出誤差。

接著，如步驟362所示，依據所算出的誤差，控制器修正對所有環境特徵點之估測位置。於步驟362中，控制器可逐一修正所有環境特徵點之位置誤差，或是同時修正所有環境特徵點之位置誤差。

如步驟363所示，根據上述步驟362所計算出的誤差，控制器修正對載體所估測之位置、姿態等資訊。

如步驟370所示，經由估測-測量-修正結果，可以對載體定位，估測載體的3D姿態，並建構環境地圖(含有環境特徵點)。

如果在控制器欲進行步驟310的估測時，控制器已得到視覺感測元件之修正後結果(亦即步驟370已完成)，則控制器會依據視覺感測元件之修正後結果來進行步驟310的估測。相似地，如果在控制器欲進行步驟340的估測時，控制器已得到慣性感測元件之修正後結果(亦即步驟330已完成)，則控制器會依據慣覺感測元件之修正後結果來進行步驟340的估測。

於本實施例中，如果慣性感測元件與視覺感測元件之操作速度不同步的話(通常，慣性感測元件的操作速度較快而視覺感測元件的操作速度較慢)，則於第3圖中，步驟310~330之操作不同於步驟340~370。

底下將詳述本實施例之詳細做法。利用慣性感測元件來定位之狀態定義如下：

x_t =g (x _{t-1 ,} u ₁ )+ε₁

z ₁ =h (x _t )+δ₁

假設運動模型(Motion Model)為X _t ⁼ g (X _{t -1} ,U _t )+ε _t ，則載體狀態為：

X _t =[X _{G , t} V _{x , t} A _{x , t} Y _{G , t} V _{y , t} A _{y , t} Z _{G , t} V _{z , t} A _{z , t} e _{0, t} e _{1, t} e _{2, t} e _{3, t} ] ^T

其中，[X _{G , t} Y _{G , t} Z _{G , t} ] ^T 為載體在世界座標中的絕對位置，[V _{x , t} V _{y , t} v _{z , t} ] ^T 為載體在載體座標中的速度，[A _{x , t} A _{y , t} A _{z , t} ] ^T 為載體在載體座標中的加速度，[e _{0, t} e _{1, t} e _{2, t} e _{3, t} ] ^T 為載體在載體座標中的四元數(quaternion)，U _t =[a _{x , t} a _{y , t} a _{z , t} ω _{x , t} ω _{y , t} ω _{z , t} ] ^T 為載體在載體座標中的加速度與角速度。

要算出載體於t 時在世界座標中的絕對位置B _t ，需要載體於t -1時在世界座標中的絕對位置、載體上的加速規與陀螺儀所得到加速度和角速度的積分資訊，且利用四元數把載體座標資訊經由載體座標轉換成世界座標，而且，以上過程在運動模型中完成。

載體狀態的運動模型表示如下：

地圖狀態的運動模型

其中，g _x,t 為重力加速度在載體座標軸x軸的分量，g _y,t 為重力加速度在載體座標軸y軸的分量，g _z,t 為重力加速度在載體座標軸z軸的分量，ε _t 為感測器雜訊，R ₁₁ ~R ₃₃ 為方向餘弦矩陣(Direction Cosine Matrix)內的參數。

經由以上的運動模型，可算出載體在空間中的位置[X _G,t Y _G,t Z _G,t ] ^T 、載體在載體座標中的加速度[A _x,t A _y,t A _z,t ] ^T 、載體在載體座標中的速度[V _x,t V _y,t V _z,t ] ^T 與載體的四元數[e _{0 ,t} e _{1 ,t} e _{2 ,t} e _{3 ,t} ]^T 。

所算出的載體狀態會包含加速規與陀螺儀感測器的雜訊，故需要修正其誤差。所以，於本實施例，以視覺感測元件當做感測模型(Sensor Model)，以修正加速規與陀螺儀估測出來的物體狀態。單眼相機(mono camera)之成本較低，但需要透過時間差來計算出相機與特徵點間的相對距離。

以下說明慣性感測元件(IMU)搭配單眼相機之實施方式。結合慣性感測元件(IMU)搭配單眼相機之系統狀態(System State)表示如下。

系統狀態包含相機狀態和所有環境特徵點的狀態。其數學表示為一維向量，向量的大小幾乎和環境特徵點的個數成正比。

相機狀態(Camera State)

相機狀態包含相機的位置向量、旋轉指向向量、直線移動速度向量、旋轉角速度向量。相機狀態的數學形式為13×1的一維向量。

為相機的位置向量；為相機的旋轉朝相四元數向量；為相機的直線移動速度向量；以及為相機的旋轉角速度向量。

特徵狀態(Feature State)

特徵狀態紀錄環境特徵點在空間中的座標，一般以3×1的一維向量表示。

運動模型(Motion Model)

運動模型定義每張畫面之間的系統狀態改變，其為二階運動方程式，表示如下。

其中，為直線加速度；為旋轉加速度；^{Δ t} 為狀態更新時間間隔；以及Q (x )為尤拉角轉四元數的轉換函式。

觀測模型(Observation Model)

如果採用不同的觀測模型且觀測向量的維度越高，則卡爾曼增益(Kalman Gain)的維度也隨之線性增加。

在第一實施例中，觀測模型乃是定義如何從相機的姿態位置與空間特徵點的座標推算出特徵點在相機中的成像位置。

其中，(u ₀ ,v ₀ )為畫面的中心座標(以左上角座標為(0,0))；(k _u ,k _v )分別為像素的長度倒數與寬度倒數；K 為相機廣角扭曲係數；以及為環境特徵點相對於相機的座標。

系統共變數矩陣(System Covariance Matrix)

系統共變數矩陣定義出，在系統狀態向量中，每個元素彼此之間的變異關係，也包含每個元素的自我不確定性。雖然系統共變數矩陣的大小和環境特徵點的個數成平方比，但在對一個環境特徵點進行估測-測量-修正(Predict-Measure-Update)時，只針對相機和目前正被估測-測量-修正的單一特徵點的共變數次矩陣(Covariance Submatrix)進行運算。系統共變數矩陣表示如下：

其中，各參數代表共存的n個特徵點，n可以為任意正整數。

第二實施例：慣性感測元件+視覺感測元件(立體視覺相機)

於第二實施例中，視覺感測元件120為立體視覺相機(stereo vision camera)，且其同樣配置於載體100的表面上，以拍攝室內的環境特徵點。立體視覺相機之成本較高，但在同一時刻就可算出相機與環境間的景深資訊。

立體視覺相機的觀測模型

原則上，第二實施例之模型類似/相同於第一實施例，不過，第二實施例之立體視覺相機的觀測模型不同於第一實施例之單眼相機之觀測模型。

於第二實施例中，立體視覺相機的觀測模型定義出如何從相機的姿態位置與環境特徵點的座標推算出環境特徵點在左右相機中的成像位置。在此假設左右相機內部參數相同，且其內部參數彼此為平移。

其中，分別為左右相機的畫面中心座標(以左上角座標為(0,0))；(k _u ,k _v )分別為像素的長度倒數與寬度倒數，在立體視覺中假設左右相機的這兩參數相同；K 為相機廣角扭曲係數，在立體視覺中假設左右相機的此參數相同；以及與分別為環境特徵點相對於左右相機的座標，其關係如下：

其中，T _{R 2 L} 為右相機座標系平移至左相機座標系的平移向量。

綜上，於第一實施例中，視覺感測元件更可量測出環境特徵點在相機中投影成像的畫面座標(u,v)。於第二實施例中，視覺感測元件的量測狀態有兩種可能定義方式：將第一實施例中的單一單眼相機擴張成兩個單眼相機，因此可得到左右兩組(u,v)；或者將立體視覺相機當成單一感測器，量測環境特徵點在空間中的(x,y,z)座標。

於本發明其他實施例中，視覺感測元件可為一個或複數個單眼像機或一個或多個全像鏡頭相機等。

綜上所述，於本發明上述兩個實施例與其他可能實施例中，可融合多重感測元件之感測結果之估測(載體姿態、速度、角速度、位置、環境特徵點的位置與相對距離)。利用慣性感測元件(陀螺儀與加速規)與視覺感測元件(單眼相機或是立體視覺相機)，以定位出移動載體在空間中的相對位置與環境特徵點相對於移動載體的位置，達到狀態估測的目的。可利用兩種感測器的互補性，大幅減少運算複雜度，以擴增對載體的動態偵測範圍，達成3D移動載體的定位(甚至可達到高速即時定位)。

此外，本發明實施例亦可結合於室外定位系統，以補償室外定位系統之不足。

綜上所述，雖然本發明已以實施例揭露如上，然其並非用以限定本發明。本發明所屬技術領域中具有通常知識者，在不脫離本發明之精神和範圍內，當可作各種之更動與潤飾。因此，本發明之保護範圍當視後附之申請專利範圍所界定者為準。

100．．．載體

110．．．慣性感測元件

120‧‧‧視覺感測元件

130‧‧‧控制器

140‧‧‧顯示單元

210~230‧‧‧環境特徵點

241~243‧‧‧載體之姿態與位置

251‧‧‧實際畫面

210A~230A‧‧‧環境特徵點之實際位置

252‧‧‧估測畫面

210B~230B‧‧‧環境特徵點之估測位置

253‧‧‧實際畫面

210C~230C‧‧‧環境特徵點之實際位置

E1~E3‧‧‧估測誤差

310~370‧‧‧步驟

第1圖顯示根據本發明第一實施例之系統示意圖。

第2圖顯示本第一實施例如何結合慣性感測元件與視覺感測元件來估測載體的狀態。

第3圖顯示根據本發明第一實施例之流程圖。

310~370．．．步驟

Claims (13)

1. 一種定位載體、估測載體姿態與建地圖之系統，包括：一慣性感測元件，測量該載體的一移動狀態與一旋轉狀態；一視覺感測元件，配置於該載體的一表面上以拍攝該載體所在的一室內環境內的至少一環境特徵點；以及一控制器，控制該慣性感測元件與該視覺感測元件，接收該慣性感測元件之一感測結果與該視覺感測元件之一感測結果以估測該載體之一姿態資訊、該載體之一位置資訊與一速度資訊，並建構含有該環境特徵點的一地圖；其中，該控制器根據該慣性感測元件與該視覺感測元件之一之一修正後感測結果而估測，之後，該控制器令該慣性感測元件與該視覺感測元件之另一感測並且據以修正該載體之該姿態資訊、該載體之該位置資訊、該速度資訊與該地圖；在命令該慣性感測元件進行量測之前，該控制器估測該載體的該姿態資訊、該位置資訊與該速度資訊；如果在該控制器估測之前，該控制器已計算出該視覺感測元件之該修正後感測結果，則該控制器依據該視覺感測元件之該修正後感測結果進行估測；在該控制器之控制下，該慣性感測元件測量該載體的該移動狀態與該旋轉狀態，並將該感測結果回傳給該控制器；以及 根據該慣性感測元件之該感測結果，該控制器修正該姿態資訊、該位置資訊與該速度資訊。
2. 如申請專利範圍第1項所述之系統，其中，該移動狀態至少包括一加速度資訊，與該旋轉狀態至少包括一角速度資訊。
3. 如申請專利範圍第2項所述之系統，其中，在命令該視覺感測元件進行測量之前，該控制器先估測該載體的該姿態資訊、該位置資訊與該速度資訊；如果在該控制器估測之前，該控制器已計算出該慣性感測元件之該修正後感測結果，則該控制器依據該慣性感測元件之該修正後結果進行估測；在該控制器之控制下，該視覺感測元件拍攝該載體所在室內的該環境特徵點畫面，並將該感測結果回傳給該控制器；以及根據該視覺感測元件之該感測結果，該控制器修正該姿態資訊、該位置資訊與該速度資訊。
4. 如申請專利範圍第3項所述之系統，其中，根據該視覺感測元件所拍攝之一實際畫面，該控制器比較該環境特徵點之一估測位置與一實際位置間之差異，以計算出一誤差；依據所算出的該誤差，該控制器修正對該環境特徵點之該估測位置，並更新該地圖；以及根據該誤差，該控制器修正該姿態資訊、該位置資訊與該速度資訊。
5. 如申請專利範圍第4項所述之系統，其中，如果 該室內環境具有複數環境特徵點，則該控制器逐一修正該些環境特徵點之各別估測位置，或是該控制器同時修正該些環境特徵點之各別估測位置。
6. 如申請專利範圍第1項所述之系統，其中，該視覺感測元件包括下列之至少一者或其任意組合：一個或複數個單眼相機，一個或複數個全像鏡頭相機。
7. 如申請專利範圍第1項所述之系統，其中，該慣性感測元件包括可為下列之至少一者或其任意組合：一加速規、一陀螺儀、一轉速計陣列。
8. 如申請專利範圍第1項所述之系統，更包括：一顯示單元，連接於該控制器，在該控制器之控制下，該顯示單元產生一外界互動反應。
9. 如申請專利範圍第8項所述之系統，其中，該顯示單元顯示一影音表達語音、一音樂、一預錄聲音。
10. 一種定位載體、估測載體姿態與建地圖之方法，包括：利用一慣性感測元件以測量該載體的一移動狀態與一旋轉狀態；利用一視覺感測元件以拍攝該載體所在的一室內環境內的至少一環境特徵點；以及根據該慣性感測元件與該視覺感測元件之一之一修正後感測結果而估測，令該慣性感測元件與該視覺感測元件之另一感測並且據以修正該載體之一姿態資訊、該載體之一位置資訊、一速度資訊與一地圖；其中， 在該慣性感測元件量測之前，估測該載體的該姿態資訊、該位置資訊與該速度資訊；如果在該估測步驟之前，已計算出該視覺感測元件之該修正後感測結果，則依據該視覺感測元件之該修正後感測結果進行估測；該慣性感測元件測量該載體的該移動狀態與該旋轉狀態，並回傳該感測結果；以及根據該慣性感測元件之該感測結果，修正該姿態資訊、該位置資訊與該速度資訊。
11. 如申請專利範圍第10項所述之方法，其中，在該視覺感測元件測量之前，先估測該載體的該姿態資訊、該位置資訊與該速度資訊；如果在該估測步驟之前，已計算出該慣性感測元件之該修正後感測結果，則依據該慣性感測元件之該修正後結果進行估測；該視覺感測元件拍攝該載體所在室內的該環境特徵點畫面，並回傳該感測結果；以及根據該視覺感測元件之該感測結果，修正該姿態資訊、該位置資訊與該速度資訊。
12. 如申請專利範圍第11項所述之方法，其中，根據該視覺感測元件所拍攝之一實際畫面，比較該環境特徵點之一估測位置與一實際位置間之差異，以計算出一誤差；依據所算出的該誤差，修正對該環境特徵點之該估測位置，並更新該地圖；以及 根據該誤差，修正該姿態資訊、該位置資訊與該速度資訊。
13. 如申請專利範圍第12項所述之方法，其中，如果該室內環境具有複數環境特徵點，則逐一修正該些環境特徵點之各別估測位置，或是同時修正該些環境特徵點之各別估測位置。