TWI681168B

TWI681168B - 應用於ｓｌａｍ系統的標記點參數化更新方法及其裝置

Info

Publication number: TWI681168B
Application number: TW105119121A
Authority: TW
Inventors: 鄧進宏; 高峻; 蔡侑庭; 葉奕成; 徐丹
Original assignee: 元智大學
Priority date: 2016-06-17
Filing date: 2016-06-17
Publication date: 2020-01-01
Also published as: TW201800721A

Abstract

本發明提供一種應用於SLAM系統的標記點參數化更新方法，其動態地更新錨點(anchor point)、根據更新後的錨點重新參數化標記點，並同時更新標記點的共變異矩陣(covariance matrix)。據此，應用於SLAM系統的標記點參數化更新方法及其裝置可以更準確地推估標記點在三維空間中的位置以及裝置在三維空間中移動的軌跡、位置及方向。

Description

應用於SLAM系統的標記點參數化更新方法及其裝置

本發明提供一種標記點參數化更新方法及其裝置，且特別是關於一種應用於SLAM系統的標記點參數化更新方法及其裝置。

同步定位與地圖構建(simultaneous localization and mapping,SLAM)系統的目的在於利用感測器所獲得的資訊來推估裝置在空間中移動的軌跡及位置。解決上述問題的一個方法稱之為EKF(extended Kalman filter)。此方法會維護一個系統狀態(state)，此系統狀態通常包含裝置當下的位置、速度、方向，以及空間中一些點(稱之為標記點(landmark))的三維空間位置。

一般來說，感測器所獲得的資訊皆有誤差，且在現有技術中，裝置係透過攝影機的畫面來進行修正。更進一步來說，假設裝置上的攝影機拍攝了當下的畫面，並在畫面中偵測到了多個特徵點。裝置會以當時的位置為基準點(稱之為錨點(anchor point))搭配特徵點的影像座標來推估這些特徵點在三維空間中的位置，即稱為標記點，且將這些標記點參數化來作為表示，如倒數深度表示法(inverse depth parametrization,IDP)，即以仰角、方位角與深度值倒數來表示標記點。

然而，在SLAM系統中，習知錨點是以裝置在某個時刻下的位置進行定義，其位置雖可輕微變化但大體仍是以當下的位置為基準，此表示其無法適應於環境的變化因而可能推估出不準確的標記點。因此，若可以在適當的時機更新錨點，將可得到更準確的標記點，使得推估裝置在空間中移動的軌跡及位置可以更準確。

本發明實施例提供一種應用於SLAM系統的標記點參數化更新方法，適用於一裝置，且於行進間紀錄裝置之一裝置位置、一方位、複數個標記點及每個標記點對應的錨點。每一個標記點表示一畫面中對應的一特徵點在一三維空間中的位置資訊。標記點參數化更新方法包括如下步驟：根據對應的錨點與標記點的參數化數值，推估未更新錨點的每一個標記點在三維空間中的位置資訊；於每一個標記點中，判斷目前的裝置位置是否適合作為對應的錨點；若判斷目前的裝置位置適合作為對應的錨點，將對應的錨點更新為目前的裝置位置，根據更新後的錨點與未更新錨點的標記點的參數化數值或者與對應的特徵點之一影像座標產生一目前標記點的參數化數值，且根據未更新錨點的標記點的參數化數值與目前標記點的參數化數值更新一共變異矩陣以利後續用於標記點及裝置方位的修正；若判斷目前的裝置位置不適合作為對應的錨點，不更新對應的錨點，且不更新共變異矩陣。處理器將未更新錨點的標記點的參數化數值作為目前標記點的參數化數值，並以現有的共變異矩陣進行下一階段標記點及裝置方位的修正。

本發明實施例提供一種應用於SLAM系統的標記點參數化更新裝置。標記點參數化更新裝置包括一攝像模組與一處理器。攝像模組偵測於行進間之一畫面中的多個特徵點。處理器電連接攝像模組，於行進間紀錄一裝置位置、一方位、複數個標記點及每一該標記點對應的一錨點，每一該標記點表示一畫面中對應的一特徵點在一三維空間中的位置資訊，且執行以下步驟：根據對應的錨點與標記點的參數化數值推估未更新錨點的每一個標記點在三維空間中的位置資訊；以及於每一個標記點中判斷目前的裝置位置是否適合作為對應的錨點。若處理器判斷目前的裝置位置適合作為對應的錨點，處理器將對應的錨點更新為目前的裝置位置。處理器根據更新後的錨點與未更新該錨點的該標記點的參數化數值或者與對應的該特徵點之一影像座標產生一目前標記點，且根據未更新錨點的標記點的參數化數值與目前標記點的參數化數值更新一共變異矩陣以利後續用於標記點及裝置方位的修正。若處理器判斷目前的裝置位置不適合作為對應的錨點，處理器不更新對應的錨點，且不更新共變異矩陣。處理器將未更新錨點的標記點的參數化數值作為目前標記點的參數化數值，並以現有的共變異矩陣進行下一階段標記點及裝置方位的修正。

綜合以上所述，本發明實施例所提供的應用於SLAM系統的標記點參數化更新方法及其裝置，其動態地更新錨點、根據更新後的錨點重新參數化標記點，並同時更新標記點的共變異矩陣。據此，應用於SLAM系統的標記點參數化更新方法及其裝置可以更準確地推估裝置在三維空間中的位置以及裝置在三維空間中移動的軌跡、位置及方向。

為使能更進一步瞭解本發明之特徵及技術內容，請參閱以下有關本發明之詳細說明與附圖，但是此等說明與所附圖式僅係用來說明本發明，而非對本發明的權利範圍作任何的限制。

100‧‧‧標記點參數化更新裝置

110‧‧‧攝像模組

120‧‧‧處理器

A、B、C、D‧‧‧特徵點

S310、S320、S330、S340‧‧‧步驟

圖1是本發明一實施例之應用於SLAM系統的標記點參數化更新裝置的示意圖。

圖2是本發明一實施例之畫面中的特徵點的示意圖。

圖3是本發明一實施例之應用於SLAM系統的標記點參數化更新方法的流程圖。

在下文中，將藉由圖式說明本發明之各種例示實施例來詳細描述本發明。然而，本發明概念可能以許多不同形式來體現，且不應解釋為限於本文中所闡述之例示性實施例。此外，圖式中相同參考數字可用以表示類似的元件。

首先，請同時參考圖1，其顯示本發明一實施例之應用於SLAM系統的標記點參數化更新裝置的示意圖。如圖1所示，標記點參數化更新裝置100為應用於SLAM系統，以推估標記點參數化更新裝置100在空間中移動的軌跡及位置。在本實施例中，標記點參數化更新裝置100可為電腦主機，平板電腦或其他電子裝置，本發明對此不作限制。更進一步來說，標記點參數化更新裝置100可設置在一移動平台上，以於行進間推估其在三維空間中移動的軌跡及位置。

標記點參數化更新裝置100包括一攝像模組110與一處理器120。處理器120於行進間計算一裝置位置與一方位。在本實施例中，處理器120事先設定好裝置100行進一次的速度與方位，以據此推估於行進間的裝置位置與方位。而在其他實施例中，處理器120可具有一感測模組(未繪於圖式中)。感測模組感測裝置100於行進間之裝置位置與方位。更進一步來說，感測模組具有一三軸加速度計與一陀螺儀(未繪於圖式中)，且可依照實際狀況來配置三軸加速度計與陀螺儀的位置。三軸加速度計與陀螺儀為分別感測裝置100在行進間的加速度與方位。而加速度將透過積分運算產生標記點參數化更新裝置100的裝置位置。感測模組亦可根據實際狀況額外設置其他感測器，如速度感測器，以供處理器120進行後續處理。舉例來說，處理器120在時間為0.01秒時，推估標記點參數化更新裝置100之裝置位置p、速度v與方位q如下<表一>所示之系統狀態。

請同時參考圖2，其顯示本發明一實施例之畫面中的特徵點的示意圖。如圖1-2所示，攝像模組110為用來偵測標記點參數化更新裝置100於行進間之畫面Fr中的複數個特徵點A、B、C與D，並將特徵點A-D在畫面Fr中的影像座標提供給處理器120，以供處理器120估測特徵點A-D之影像座標在三維空間中的位置資訊。而每一個特徵點A-D之影像座標在三維空間中的位置資訊係由一個標記點來作表示(以下會作詳細說明)。在本實施例中，攝像模組110係透過角點偵測(Corner Detection)演算法來尋找畫面Fr中的特徵點A-D，且亦可透過其他演算法來尋找畫面Fr中的特徵點，本發明對此不作限制。舉例來說，攝像模組110找到4個特徵點A-D，且特徵點A-D在畫面Fr中的影像座標如下<表二>所示之系統狀態。

處理器120電連接攝像模組110，且用以執行下列步驟，以根據每一個特徵點A-D與對應的錨點來推估每個特徵點A-D在三維空間中的位置資訊。而每一個特徵點A-D之影像座標在三維空間中的位置資訊將以標記點A1、B1、C1與D1作表示。有關每一個特徵點A-D之影像座標在三維空間中的位置資訊將以標記點A1、B1、C1與D1作表示為所屬領域具有通常知識者所知，故在此不再贅述。本實施例的標記點A1-D1係為參數化後的標記點，且以倒數深度表示法(inverse depth parametrization,IDP)來作表示，以方便處理器120計算。而標記點A1-D1亦可以其他參數化表示法，如AHP(anchored homogeneous point)參數化表示法，本發明對此不作限制。此外，每個標記點A1-D1分別有一個錨點，且每個錨點分別為每個標記點A1-D1對應於標記點參數化更新裝置100的基準點。而有關錨點之定義為所屬領域具有通常知識者所知，故在此不再贅述。承接上述例子，處理器120在時間為0.01秒時，系統狀態中的裝置位置p、速度v、方位q、錨點m與標記點A1-D1的參數化數值如下<表三>所示。

請同時參考圖3，其顯示本發明一實施例之應用於SLAM系統的標記點參數化更新方法的流程圖。在標記點參數化更新裝置100行進一段時間後，處理器120將推估目前(如時間為0.05秒)的系統狀態。首先，處理器120將根據對應的錨點(如表三之錨點m=[0,0,0.020045])與對應的特徵點A-D的標記點A1-D1的參數化數值推估未更新錨點的每一個標記點E1、F1、G1與H1的參數化數值(步驟S310)。

而未更新錨點的每一個標記點E1、F1、G1與H1的參數化數值的計算方式將根據對應的錨點與對應的特徵點的標記點A1-D1的參數化數值並透過例如卡曼濾波(Extended Kalman Filter,EKF)演算法來計算，且在此不再贅述。承接上述例子，處理器120在時間為0.05秒時，推估的系統狀態中的裝置位置p、速度v、方位q、錨點m與未更新錨點的標記點E1-H1的參數化數值如下<表四>所示。

接著，處理器120將在每一個標記點A1-D1中，進一步判斷目前的裝置位置是否適合作為對應的錨點(步驟S320)。在本實施例中，處理器120將在每一個標記點A1-D1中，根據未更新錨點的標記點、未更新錨點、與目前的裝置位置之間的關係來判斷目前的裝置位置是否適合作為對應的錨點。更進一步來說，未更新錨點的標記點、未更新錨點與目前的裝置位置之間的關係為一距離關係。

以未更新錨點的標記點E1、未更新錨點m=[0,0,0.020045]與目前的裝置位置之間的距離關係來作說明。因此，若未更新錨點的標記點E1與目前的裝置位置之間的距離小於等於未更新錨點的標記點E1與未更新錨點m=[0,0,0.020045]之間的距離，未更新錨點的標記點E1距離目前的裝置位置較近，表示目前的裝置位置適合作為對應的錨點；反之，若未更新錨點的標記點E1與目前的裝置位置之間的距離大於未更新錨點的標記點E1與未更新錨點m=[0,0,0.020045]之間的距離，未更新錨點的標記點E1距離目前的裝置位置較遠，表示目前的裝置位置不適合作為對應的錨點。

在每一個標記點A1-D1中，若處理器120判斷目前的裝置位置適合作為對應的錨點，處理器120接著將執行步驟S330，即處理器120將對應的錨點更新為目前的裝置位置，且根據更新後的錨點與未更新錨點的標記點E1-H1的參數化數值，或者根據更新後的錨點與對應的特徵點之一影像座標產生一目前標記點的參數化數值，以取得目前的時間下，畫面Fr中對應的特徵點在三維空間中的位置資訊。再來，處理器120將根據未更新錨點的標記點的參數化數值與目前標記點的參數化數值更新一共變異矩陣。有關處理器120重新參數化對應的標記點的計算方法為所屬領域具有通常知識者所熟知，故在此不再贅述。

承接<表四>例子，未更新錨點的標記點E1=[1.124891,-2.356195,0.296355]為參數化後的標記點。處理器120利用對應的錨點m=[0,0,0.020045]將未更新錨點的標記點E1轉換為三維空間中的座標=[-1.060748,-1.060748,3.177802]。代表未更新錨點的標記點E1在三維空間中的座標=[-1.060748,-1.060748,3.177802]與對應的錨點m=[0,0,0.020045]之間的距離為

。而代表未更新錨點的標記點E1在三維空間中的座標=[-1.060748,-1.060748,3.177802]與目前的裝置位置p=[0,0,0.101125]之間距離為

。故由上述可知，未更新錨點的標記點E1與目前的裝置位置之間的距離小於等於未更新錨點的標記點E1與前一個錨點之間的距離。處理器120判斷未更新錨點的標記點E1距離目前的位置較近，表示目前的裝置位置適合作為對應的錨點。

接著，處理器120遂將對應的錨點m=[0,0,0.020045]更新為目前的裝置位置，即更新後的錨點m為[0,0,0.101125]。而處理器120將根據更新後的錨點m=[0,0,0.101125]與未更新錨點的標記點E1的參數化數值產生一目前標記點E2，意即處理器120將根據更新後的錨點m=[0,0,0.101125]並透過倒數深度表示法(IDP)重新參數化未更新錨點的標記點E1以產生目前標記點E2的參數化數值，以取得目前的時間下，畫面Fr中對應的特徵點在三維空間中的位置資訊。又或者處理器120亦可根據更新後的錨點m=[0,0,0.101125]與對應的特徵點A之影像座標產生目前標記點E2的參數化數值。而對於其他未更新錨點的標記點F1-H1，處理器120同樣可以上述方法重新參數化未更新錨點的標記點F1-H1，以分別產生一目前標記點F2-H2，故在此不再贅述。

如下<表五>所示之處理器120在時間為0.05秒時，重新參數化未更新錨點的標記點E1-H1後的系統狀態以產生目前標記點E2-H2。而有關目前標記點E2-H2的參數化數值的計算方式大致上類似於<表四>之未更新錨點的參數化數值的標記點E1-H1的計算方式，故在此不再贅述。

在EKF-SLAM系統中，系統會維護代表系統狀態誤差的共變異矩陣。因此，接下來，處理器120將根據未更新錨點的標記點E1-H1的共變異矩陣產生代表目前標記點E2-H2的共變異矩陣，以修正整個系統狀態，其詳細說明如下。處理器120將根據未更新錨點的標記點E1-H1的參數化數值與目前標記點E2-H2的參數化數值計算代表目前標記點E2-H2的共變異矩陣，以據此修正整個系統狀態。更進一步來說，未更新錨點的標記點E1-H1的參數化數值與目前標記點E2-H2的參數化數值之間的數值變化關聯程度(即E2-H2相對於E1-H1間數值的偏微分)可用來計算一矩陣G。因此，共變異矩陣P⁺可整理為以下形式：P⁺=GP^-G^T其中，P⁺為錨點更新後的系統狀態共變異矩陣，P^-為錨點更新前的系統狀態共變異矩陣，G^T則為矩陣G的轉置矩陣。據此，處理器120將可透過矩陣G來計算錨點更新後的共變異矩陣P⁺。

承接<表四>與<表五>之例子，為了方便說明，以下僅以重新參數化未更新錨點的標記點E1以產生目前標記點E2的參數化數值來作說明。Φ(p^-,x^-)代表錨點更新前根據標記點E1的參數化數值所計算而得標記點三維空間位置的函數，其中p^-為錨點m=[0,0,0.020045]，x^-為未更新錨點的標記點E1的參數化數值[1.124891,-2.356195,0.296355]。Φ(p⁺,x⁺)代表錨點更新後之標記點E2的參數化數值所計算而得標記點三維空間位置的函數，其中p⁺為更新後的錨點m=[0,0,0.101125]，x⁺為目前標記點E2的參數化數值[1.116900,-2356194,0.29214]。

而不管是未更新錨點的標記點E1或是目前標記點E2的表示方式，其皆代表空間中同一個標記點，進而形成以下等式：Φ(p^-,x^-)=Φ(p⁺,x⁺)其中，函數Φ表示從標記點E1的參數化資訊(即(p^-,x^-)與(p⁺,x⁺))所計算出的標記點三維空間中的位置資訊。將上述方程式展開之後對等號兩邊取變數x^-的偏微分可得到如下方程式：

其中，

與

。而Y的定義如下：

其中，W為標記點參數化更新前後的數值變化關聯程度。而經簡化後，共變異矩陣P⁺遂可整理為如下形式：

其中，I_n與I_m分別代表n階與m階的單位矩陣，且其大小為根據系統狀態中的參數個數而定。因此，n為10(即<表五>中位置p、速度v及方位q的變數個數總和)，且m為9(即<表五>中標記點B1-D1之更新位置資訊F2-H2的變數個數總和)。

在矩陣G中，W即為前述的

，而M代表標記點參數化數值對系統狀態中裝置位置、速度與方向的偏微分。換言之

，其中s代表<表一>中的位置p、速度v及方位q所組合而成的變數。

而若處理器120判斷目前的裝置位置不適合作為對應的錨點，處理器120接著將執行步驟S340，即處理器120不更新錨點，不更新共變異矩陣，且將未更新錨點的標記點的參數化數值作為目前標記點的參數化數值，以取得目前的時間下，畫面Fr中對應的特徵點在三維空間中的位置資訊。有關處理器120重新參數化對應的標記點的計算方法為所屬領域具有通常知識者所熟知，故在此不再贅述。

而同樣以<表四>為例，若未更新錨點的標記點E1與目前的裝置位置p=[0,0,0.101125]之間的距離大於未更新錨點的標記點E1與前一個錨點m=[0,0,0.020045]之間的距離。處理器120將判斷出未更新錨點的標記點E1距離目前的裝置位置p較遠，表示目前的裝置位置p不適合作為對應的錨點。

此時，處理器120將不更新對應的錨點m，即以前一個錨點m=[0,0,0.020045]作為目前的錨點。而對於其他未更新錨點的標記點F1-H1，處理器120同樣以此方法不更新對應的錨點m，即以前一個錨點m=[0,0,0.020045]作為目前的錨點，故在此不再贅述。

故由上述可知，本發明實施例所提供的應用於SLAM系統的標記點參數化更新方法及其裝置，其處理器120將不斷地判斷標記點參數化更新裝置100目前的位置是否適合作為錨點(如利用未更新錨點的標記點、未更新錨點與目前的裝置位置之間的距離關係作為判斷依據)，以動態地更新錨點，根據更新後的錨點重新參數化標記點，並同時更新標記點的共變異矩陣。據此，應用於SLAM系統的標記點參數化更新方法及其裝置可以更準確地推估標記點在三維空間中的位置以及裝置在三維空間中移動的軌跡、位置及方向。

以上所述僅為本發明之實施例，其並非用以侷限本發明之專利範圍。

S310、S320、S330、S340‧‧‧步驟

Claims

一種應用於同步定位與地圖構建(simultaneous localization and mapping,SLAM)系統的標記點參數化更新方法，適用於一裝置，且於行進間紀錄該裝置之一裝置位置、一方位、複數個標記點及每一該標記點對應的一錨點(anchor point)，每一該標記點表示一畫面中對應的一特徵點的一影像座標在一三維空間中的位置資訊，且該標記點參數化更新方法包括：根據對應的該錨點與該標記點的參數化數值，推估未更新該錨點的每一該標記點在該三維空間中的位置資訊；於每一該標記點中，判斷目前的該裝置位置是否適合作為對應的該錨點；若判斷目前的該裝置位置適合作為對應的該錨點，將對應的該錨點更新為目前的該裝置位置，根據更新後的該錨點與未更新該錨點的該標記點的參數化數值或者與對應的該特徵點之一影像座標產生一目前標記點的參數化數值，且根據未更新該錨點的該標記點的參數化數值與該目前標記點的參數化數值更新一共變異矩陣；以及若判斷目前的該裝置位置不適合作為對應的該錨點，不更新對應的該錨點，不更新該共變異矩陣，且將未更新該錨點的該標記點的參數化數值作為該目前標記點的參數化數值；其中，於每一該標記點判斷目前的該裝置位置是否適合作為對應的該錨點的步驟中，係以未更新該錨點的該標記點、未更新該錨點與目前的該裝置位置之間的一距離關係作為判斷依據；若未更新該錨點的該標記點與目前的該裝置位置之間的距離小於等於未更新該錨點的該標記點與未更新該錨點之間的距離，表示目前的該裝置位置適合作為對應的該錨點；以及若未更新該錨點的該標記點與目前的該裝置位置之間的距離大於未更新該錨點的該標記點與未更新該錨點之間的距離，表示目前的該位置不適合作為該錨點。
如請求項1之標記點參數化更新方法，其中，每一該標記點的參數化數值係以一倒數深度表示法(inverse depth parametrization,IDP)表示。
如請求項1之標記點參數化更新方法，其中，該共變異矩陣為由一矩陣表示，且該矩陣關聯於該裝置之該裝置位置、該方位與每一該標記點的參數化數值。
一種應用於同步定位與地圖構建(simultaneous localization and mapping,SLAM)系統的標記點參數化更新裝置，包括：一攝像模組，偵測於行進間之一畫面中的複數個特徵點；一處理器，電連接該攝像模組，於行進間紀錄一裝置位置、一方位、複數個標記點及每一該標記點對應的一錨點，每一該標記點表示一畫面中對應的一特徵點的一影像座標在一三維空間中的位置資訊，且執行以下步驟：根據對應的該錨點與該標記點的參數化數值，推估未更新該錨點的每一標記點在該三維空間中的位置資訊；於每一該標記點中，判斷目前的該裝置位置是否適合作為對應的該錨點；若判斷目前的該裝置位置適合作為對應的該錨點，該處理器將對應的該錨點更新為目前的該裝置位置，根據更新後的該錨點與未更新該錨點的該標記點的參數化數值或者與對應的該特徵點之一影像座標產生一目前標記點的參數化數值，且根據未更新該錨點的該標記點的參數化數值與該目前標記點的參數化數值更新一共變異矩陣；以及若判斷目前的該裝置位置不適合作為對應的該錨點，該處理器不更新對應的該錨點，不更新該共變異矩陣，且將未更新該錨點的該標記點的參數化數值作為該目前標記點的參數化數值；其中，該處理器係以未更新該錨點的該標記點、未更新該錨點與目前的該裝置位置之間的一距離關係作為判斷依據；若未更新該錨點的該標記點與目前的該裝置位置之間的距離小於等於未更新該錨點的該標記點與未更新該錨點之間的距離，表示目前的該裝置位置適合作為對應的該錨點；以及若未更新該錨點的該標記點與目前的該裝置位置之間的距離大於未更新該錨點的該標記點與未更新該錨點之間的距離，表示目前的該位置不適合作為該錨點。
如請求項4之標記點參數化更新裝置，其中，該處理器具有一感測模組，感測於行進間之該裝置位置與該方位。
如請求項4之標記點參數化更新裝置，其中，該共變異矩陣為由一矩陣表示，且該矩陣關聯於該裝置之該裝置位置、該方位與每一該標記點的參數化數值。