TW202108982A

TW202108982A - 同步定位與建圖優化方法

Info

Publication number: TW202108982A
Application number: TW108130812A
Authority: TW
Inventors: 王俞芳; 蔡宜學
Original assignee: 財團法人車輛研究測試中心
Priority date: 2019-08-28
Filing date: 2019-08-28
Publication date: 2021-03-01
Also published as: TWI736960B

Abstract

一種同步定位與建圖優化方法包含：持續感測一待定位物件的一周遭環境，以獲得在一當前與前一單位時間點的第一感測資料；對於每一單位時間點的第一感測資料，根據該第一感測資料，獲得多個物件；自該當前單位時間點的該等物件篩選出多個待追蹤物件；對於每一待追蹤物件，根據該待追蹤物件，與該前一單位時間點中對應該待追蹤物件的待比對物件，將該待追蹤物件分類為一動態物件或一靜態物件；根據分類結果，濾除所感測到之相關於該周遭環境的第二感測資料中對應於每一動態物件的資料部分；及根據經濾除的該第二感測資料定位出該待定位物件。

Description

同步定位與建圖優化方法

本發明是有關於一種同步定位與建圖方法，特別是指一種基於物件追蹤之同步定位與建圖優化方法。

現有的定位技術通常使用高精度GPS導航定位模組來進行定位。然而在環境較複雜且遮蔽物多或是天氣不好的情境下，GPS的定位準確度則會受影響。故根據環境特徵進行自我定位之同步定位與建圖(Simultaneous Localization and Mapping，簡稱SLAM )方法技術已普遍從室內的掃地機器人演變到為室外的自駕車所用。

現有的SLAM技術係透過感測器(如，光學鏡頭、雷射光達、彩色相機或深度相機)進行周遭環境的感測，並將環境狀況記錄於地圖當中，而在建立地圖的同時，根據環境特徵在地圖上定位待定位物件。然而，現有的SLAM技術在進行特徵與底圖的匹配過程中，會因動態物體存在之不確定性導致匹配時產生誤判或雜訊等情形，進而影響定位精確度。

因此，本發明的目的，即在提供一種提升定位精確度的同步定位與建圖優化方法。

於是，本發明同步定位與建圖優化方法，適用於定位一待定位物件，並藉由一設置於待定位物件上之定位系統來實施，該定位系統包含一感測單元、一同步定位與建圖單元及一電連接該感測單元與該同步定位與建圖單元的物件分類單元，該同步定位與建圖優化方法包含以下步驟：

(A)藉由該感測單元持續感測該待定位物件的一周遭環境，以獲得在一當前單位時間點與前一單位時間點之相關於該周遭環境的第一感測資料；

(B)對於每一單位時間點所獲得的第一感測資料，藉由該物件分類單元，根據該第一感測資料，獲得多個物件；

(C) 對於該當前單位時間點所對應的該等物件，藉由該物件分類單元自該等物件篩選出多個待追蹤物件；

(D)對於該當前單位時間點所對應的每一待追蹤物件，藉由該物件分類單元根據該待追蹤物件，與該前一單位時間點中對應該待追蹤物件的待比對物件，將該待追蹤物件分類為一動態物件或一靜態物件；

(E)藉由該物件分類單元將步驟(D)的分類結果傳送至該同步定位與建圖單元；及

(F) 藉由該同步定位與建圖單元，根據步驟(D)的分類結果，濾除該同步定位與建圖單元所感測到之相關於該周遭環境的第二感測資料中對應於每一動態物件的資料部分；及

(G) 藉由該同步定位與建圖單元，根據經濾除的該第二感測資料，利用一同步定位與建圖演算法，來定位出該待定位物件。

本發明的功效在於：藉由該物件分類單元將每一待追蹤物件分類為一動態物件或一靜態物件，並藉由該同步定位與建圖單元濾除該同步定位與建圖單元所感測到之相關於該周遭環境的第二感測資料中對應於每一動態物件的資料部分，接著根據經濾除的該第二感測資料利用該同步定位與建圖演算法，來定位出該待定位物件，以避免因動態物件之不確定性導致特徵匹配時產生誤判，進而提升定位精確度。

在本發明被詳細描述之前，應當注意在以下的說明內容中，類似的元件是以相同的編號來表示。

參閱圖1，本發明同步定位與建圖優化方法的一第一實施例，適用於定位一待定位物件，並藉由一設置於待定位物件上之定位系統1來實施。該定位系統1包含一感測單元11、一同步定位與建圖單元12及一電連接該感測單元11與該同步定位與建圖單元12的物件分類單元13。

該感測單元11用於持續感測該待定位物件的一周遭環境，以獲得在不同單位時間點之相關於該周遭環境的第一感測資料。在本實施例中，該感測單元11為一如光達感測器的光達模組。該光達模組用於持續掃描該待定位物件的該周遭環境，以獲得在不同單位時間點之相關於該周遭環境的點雲資料，以分別作為該等第一感測資料，每一單位時間點所獲得的點雲資料係該光達模組在一自一第一個掃描時間點至一第N個掃描時間點之預設期間內所掃描到的點雲資料。

該同步定位與建圖單元12包含一感測模組121，及一電連接該感測模組121的定位與建圖模組122。該感測模組121用於持續感測該待定位物件之該周遭環境以獲得在不同單位時間點之相關於該周遭環境的第二感測資料。該定位與建圖模組122用於根據該感測模組121所獲得的第二感測資料利用一同步定位與建圖演算法來建立相關於該周遭環境的地圖與定位該待定位物件。在本第一實施例中，該同步定位與建圖單元12之感測模組121即為該光達模組，而該第二感測資料即為該點雲資料。

該物件分類單元13用於將該當前單位時間點之第一感測資料所對應的每一待追蹤物件分類為一動態物件或一靜態物件。在本實施例中，該物件分類單元13與該定位與建圖模組122之實施態樣可整合為一具運算能力之處理器，該處理器可藉由執行一物件追蹤與分類程式及一同步定位與建圖程式以實施本發明同步定位與建圖優化方法的一第一實施例。

參閱圖1與圖2，本發明同步定位與建圖優化方法的一第一實施例包含以下步驟。

在步驟21中，該感測單元11持續感測該待定位物件的該周遭環境，以獲得在一當前單位時間點與前一單位時間點之相關於該周遭環境的第一感測資料。在本實施例中，該感測單元11即為該光達模組，該光達模組持續掃描該待定位物件的一周遭環境，以獲得在該當前單位時間點與該前一單位時間點之相關於該周遭環境的點雲資料，以分別作為該等第一感測資料。

在步驟22中，對於每一單位時間點所獲得的點雲資料，該物件分類單元13根據相關於該待定位物件自該第一個掃描時間點至該第N個掃描時間點之每一時間點之移動的一位置，與相關於該待定位物件自該第一個掃描時間點至一第N個掃描時間點之每一時間點之移動的一指向角度( Heading)，校正該點雲資料中自一第二個掃描時間點至該第N個掃描時間點所掃描到的點雲資料，以獲得校正後的點雲資料。其中，每一第一掃描時間點即為所對應之單位時間點，因此，在每一第一掃描時間點所掃描到的點雲資料無須進行校正。

參閱圖1與圖3，值得特別說明的是，步驟22包含以下子步驟。

在子步驟221中，對於每一單位時間點所獲得的點雲資料，該物件分類單元13根據相關於該待定位物件自該第一個掃描時間點至該第N個掃描時間點之每一時間點之移動的該位置，與該指向角度，獲得自該第二個掃描時間點至該第N個掃描時間點之每一時間點所對應的一平移校正矩陣與一旋轉校正矩陣。值得一提的是，該物件分類單元13先根據第一個掃描時間點的位置與該第二個掃描時間點的位置計算出從該第一個掃描時間點到該第二個掃描時間點該待追蹤物件所對應的位移(亦即，設置於該待追蹤物件上的該光達模組所對應的位移)，再根據該光達模組從該第一個掃描時間點到該第二個掃描時間點所對應的位移獲得該第二個掃描時間點所對應的該平移校正矩陣，依此類推直到該物件分類單元13先根據第N-1個掃描時間點的位置與該第N個掃描時間點的位置計算出從該第N-1個掃描時間點到該第N個掃描時間點該光達模組所對應的位移，再根據該光達模組從該第N-1個掃描時間點到該第N個掃描時間點所對應的位移獲得該第N個掃描時間點所對應的該平移校正矩陣，此外，該物件分類單元13根據該第一個掃描時間點的指向角度與該第二個掃描時間點的指向角度計算出從該第一個掃描時間點到該第二個掃描時間點該待追蹤物件所對應的轉向角度(亦即，設置於該待追蹤物件上的該光達模組所對應的轉向角度)，再根據該光達模組從該第一個掃描時間點到該第二個掃描時間點所對應的轉向角度獲得該第二個掃描時間點所對應的該旋轉校正矩陣，依此類推直到該物件分類單元13根據該第N-1個掃描時間點的指向角度與該第N個掃描時間點的指向角度計算出從該第N-1個掃描時間點到該第N個掃描時間點該待追蹤物件所對應的轉向角度，再根據該光達模組從該第N-1個掃描時間點到該第N個掃描時間點所對應的轉向角度獲得該第N個掃描時間點所對應的該旋轉校正矩陣。此外，當該待定位物件為一自駕車時，該自駕車自該第一個掃描時間點至該第N個掃描時間點之每一時間點的該位置，與該指向角度(亦即，車頭方向)係可藉由該自駕車之速率與轉角訊號利用習知的自行車模型(Bicycle Model)而獲得。

在子步驟222中，對於每一單位時間點所獲得的點雲資料，該物件分類單元13根據自該第二個掃描時間點至該第N個掃描時間點之每一時間點所對應的該平移校正矩陣與該旋轉校正矩陣，校正該點雲資料中自一第二個掃描時間點至該第N個掃描時間點所掃描到的點雲資料，以獲得校正後的點雲資料。值得一提的是，該物件分類單元13係根據該第二個掃描時間點所對應的該平移校正矩陣與該旋轉校正矩陣，校正該第二個掃描時間點所掃描到的點雲資料，並根據該第二個掃描時間點所對應的該平移校正矩陣與該旋轉校正矩陣與該第三個掃描時間點所對應的該平移校正矩陣與該旋轉校正矩陣，校正該第三個掃描時間點所掃描到的點雲資料，依此類推，該物件分類單元13根據該第二個掃描時間點至該第N個掃描時間點所對應的該平移校正矩陣與該旋轉校正矩陣，校正該第N個掃描時間點所掃描到的點雲資料。

值得特別說明的是，當該待定位物件之移動速度大於等於一預設速度閥值時，該光達模組在相鄰兩掃描時間點間之位移也會較大，因而可經由步驟22之校正來避免因該光達模組之掃描延遲而造成每一單位時間點所獲得的點雲資料有失真之情形，若該待定位物件之移動速度小於該預設速度閥值，則相鄰兩掃描時間點間之位移也隨之較小，故該光達模組之掃描延遲對每一單位時間點所獲得的點雲資料之正確性的影響不大，因此，在該待定位物件之移動速度小於該預設速度閥值的情形下，步驟22可以被省略。

在步驟23中，對於每一單位時間點所獲得的點雲資料(第一感測資料)，該物件分類單元13係藉由將該點雲資料進行點雲分群以獲得多個不同的點雲群組，並將每一點雲群組標記為一物件，以獲得每一第一感測資料所對應的多個物件。其中每一單位時間點所獲得的點雲資料，係包含該第一掃描時間點所掃描到的點雲資料及經步驟22之校正後的點雲資料。

在步驟24中，對於該當前單位時間點所對應的該等物件，該物件分類單元13自該等物件篩選出多個待追蹤物件。在本實施例中，該物件分類單元13係篩選出對應有尺寸位於一預設範圍內的該等待追蹤物件。

在步驟25中，對於該當前單位時間點所對應的每一待追蹤物件，該物件分類單元13根據該待追蹤物件，與該前一單位時間點中對應該待追蹤物件的待比對物件，將該待追蹤物件分類為一動態物件或一靜態物件。

參閱圖1與圖4，值得一提的是，步驟25還包含下子步驟。

在子步驟251中，對於每一待比對物件，該物件分類單元13根據該待比對物件所對應的點雲群組，獲得該待比對物件之一比對位置。

在子步驟252中，對於每一待追蹤物件，該物件分類單元13根據該待追蹤物件所對應的點雲群組，獲得該待追蹤物件之一當前位置。

在子步驟253中，對於每一待追蹤物件，該物件分類單元13根據該待追蹤物件之該當前位置、相關於該待定位物件在該當前單位時間點之移動的一位置與一指向角度，及相關於該待定位物件在該前一單位時間點之移動的一位置與一指向角度，估測該待追蹤物件在該前一單位時間點之前一位置。其中，該待定位物件(亦即，自駕車)在該當前單位時間點及該前一單位時間點的該位置及該指向角度係可藉由該自駕車之速率與轉角訊號利用習知的自行車模型(Bicycle Model)而獲得。值得特別說明的是，該物件分類單元13係先根據該前一單位時間點的該位置與該當前單位時間點的該位置計算出從該前一單位時間點到該當前單位時間點該待定位物件所對應的位移，再根據該待定位物件從該前一單位時間點到該當前單位時間點所對應的位移獲得該當前單位時間點所對應的該平移校正矩陣，並根據該前一單位時間點的該指向角度與該當前單位時間點的該指向角度計算出從該前一單位時間點到該當前單位時間點該待定位物件所對應的轉向角度，再根據該待定位物件從該前一單位時間點到該當前單位時間點所對應的轉向角度獲得在該當前單位時間點該待定位物件所對應的該旋轉校正矩陣。接著，該物件分類單元13根據該待定位物件之該當前位置、該當前單位時間點所對應的該平移校正矩陣與該旋轉校正矩陣，估測該待追蹤物件在該前一單位時間點之該前一位置。

在子步驟254中，對於每一待追蹤物件，該物件分類單元13根據該待追蹤物件的該前一位置，及對應該待追蹤物件之待比對物件的該比對位置，將該待追蹤物件分類為該動態物件或該靜態物件。值得一提的是，當該待追蹤物件的該前一位置與對應該待追蹤物件之待比對物件的該比對位置間之距離小於等於一預設距離閥值時，該待追蹤物件即被分類為該靜態物件；當該待追蹤物件的該前一位置與對應該待追蹤物件之待比對物件的該比對位置間之距離大於該預設距離閥值時，該待追蹤物件即被分類為該動態物件。

在步驟26中，該物件分類單元13將步驟25的分類結果傳送至該同步定位與建圖單元12。在本實施例中，該分類結果指示出每一被分類為動態物件之待追蹤物件。此外，由於在本實施例中，該同步定位與建圖單元12之感測模組121即為該光達模組，故該物件分類單元13之分類結果中所指示出的該待追蹤物件可以是該待追蹤物件所對應的點雲群組，也可以是該物件分類單元13根據該待追蹤物件所對應的點雲群組而獲得之對應於該待追蹤物件的一物件中心座標。

在步驟27中，該同步定位與建圖單元12根據步驟25的分類結果，濾除該同步定位與建圖單元所感測到之相關於該周遭環境的第二感測資料中對應於每一動態物件的資料部分。

在步驟28中，該同步定位與建圖單元12，根據經濾除的該第二感測資料，利用一同步定位與建圖演算法，來定位出該待定位物件。藉由該同步定位與建圖單元12不考慮被分類為動態物件的待追蹤物件來進行該待定位物件之定位，可避免因動態物件之不確定性導致特徵匹配時產生誤判，進而提升定位精確度。

參閱圖1與圖5，本發明同步定位與建圖優化方法的一第二實施例大致上是與該第一實施例相同，相同之處不再贅言，其中不同之處在於，在該第二實施例中，該同步定位與建圖單元12之感測模組121為一如深度相機的影像拍攝模組，該感測模組121同步於該感測單元11在不同單位時間點獲得該等第二感測資料，而該感測模組121所獲得之第二感測資料即為相關於該待定位物件之該周遭環境的影像資料，因而該物件分類單元13所傳送的分類結果中所指示出的每一待追蹤物件須經過轉換處理，以使該同步定位與建圖單元12能從該影像資料識別出對應於該分類結果中所指示出之每一待追蹤物件的影像部分，故在該第二實施例更進一步包含一步驟35及一步驟36。

在步驟35中，對於每一被分類為動態物件之待追蹤物件，該物件分類單元13根據該待追蹤物件所對應的點雲群組，獲得該待追蹤物件的一物件中心座標。

在步驟36中，對於每一被分類為動態物件之待追蹤物件，該物件分類單元13根據相關於該點雲資料之一點雲座標系與該第二感測資料(亦即，該影像資料)之像素座標系之座標系轉換的一座標轉換參數組，將該待追蹤物件的該物件中心座標轉換為一對應於該第二感測資料之像素座標系的另一物件中心座標。值得一提的是，該座標轉換參數組包含一外部參數矩陣及一內部參數矩陣，該外部參數矩陣係根據該感測單元11之架設位置與該感測模組121之架設位置利用以下公式(1)而獲得。該內部參數矩陣係根據該感測模組121之以像素為單位的焦距距離，及影像中心座標而獲得。

=

+

…(1)

其中，

為該感測模組121之架設位置，

為該感測單元11之架設位置，

為該外部參數矩陣的三軸旋轉矩陣，

為該外部參數矩陣的平移矩陣。

在步驟37中，該物件分類單元13將步驟34的分類結果傳送至該同步定位與建圖單元12。其中，所傳送的分類結果包含每一被分類為動態物件之待追蹤物件的另一物件中心座標。

在步驟 38中，該同步定位與建圖單元12根據步驟34的分類結果，濾除該同步定位與建圖單元所感測到之相關於該周遭環境的影像資料中對應於每一動態物件的資料部分。值得一提的是，第二感測資料即為該影像拍攝模組在該當前單位時間點所獲得的影像資料，此外，在本第二實施例中，由於該分類結果所指示出的該待追蹤物件為該待追蹤物件在該影像資料中的該另一物件中心座標，故該同步定位與建圖單元12即可識別出每一動態之待追蹤物件的影像部分，而不考慮對應於動態之待追蹤物件的影像部分來進行定位。

在步驟39中，該同步定位與建圖單元12，根據經濾除的該影像資料，利用該同步定位與建圖演算法，來定位出該待定位物件。

綜上所述，本發明同步定位與建圖優化方法，藉由該物件分類單元13將每一待追蹤物件分類為一動態物件或一靜態物件，以使該同步定位與建圖單元12不考慮被分類為動態物件的待追蹤物件，來定位出該待定位物件，以避免因動態物件之不確定性導致特徵匹配時產生誤判，進而提升定位精確度，故確實能達成本發明的目的。

惟以上所述者，僅為本發明的實施例而已，當不能以此限定本發明實施的範圍，凡是依本發明申請專利範圍及專利說明書內容所作的簡單的等效變化與修飾，皆仍屬本發明專利涵蓋的範圍內。

1:定位系統 11:感測單元 12:同步定位與建圖單元 121:感測模組 122:定位與建圖模組 13:物件分類單元 21~28:步驟 221~222:子步驟 251~254:子步驟 30~39:步驟

本發明的其他的特徵及功效，將於參照圖式的實施方式中清楚地呈現，其中：圖1是一方塊圖，說明實施本發明同步定位與建圖優化方法之一第一實施例的定位系統；圖2是一流程圖，說明本發明同步定位與建圖優化方法之該第一實施例；圖3是一流程圖，說明一物件分類單元如何校正每一單位時間點所獲得的點雲資料；圖4是一流程圖，說明該物件分類單元如何將一待追蹤物件分類為一動態物件或一靜態物件；及圖5是一流程圖，說明本發明同步定位與建圖優化方法之一第二實施例。

21~28:步驟

Claims

一種同步定位與建圖優化方法，適用於定位一待定位物件，並藉由一設置於待定位物件上之定位系統來實施，該定位系統包含一感測單元、一同步定位與建圖單元及一電連接該感測單元與該同步定位與建圖單元的物件分類單元，該同步定位與建圖優化方法包含以下步驟： (A)藉由該感測單元持續感測該待定位物件的一周遭環境，以獲得在一當前單位時間點與前一單位時間點之相關於該周遭環境的第一感測資料； (B)對於每一單位時間點所獲得的第一感測資料，藉由該物件分類單元，根據該第一感測資料，獲得多個物件； (C) 對於該當前單位時間點所對應的該等物件，藉由該物件分類單元自該等物件篩選出多個待追蹤物件； (D)對於該當前單位時間點所對應的每一待追蹤物件，藉由該物件分類單元根據該待追蹤物件，與該前一單位時間點中對應該待追蹤物件的待比對物件，將該待追蹤物件分類為一動態物件或一靜態物件； (E)藉由該物件分類單元將步驟(D)的分類結果傳送至該同步定位與建圖單元；及 (F)藉由該同步定位與建圖單元，根據步驟(D)的分類結果，濾除該同步定位與建圖單元所感測到之相關於該周遭環境的第二感測資料中對應於每一動態物件的資料部分；及 (G) 藉由該同步定位與建圖單元，根據經濾除的該第二感測資料，利用一同步定位與建圖演算法，來定位出該待定位物件。
如請求項1所述的同步定位與建圖優化方法，該感測單元為一光達模組，其中：在步驟(A)中，藉由該光達模組持續掃描該待定位物件的該周遭環境，以獲得在該當前單位時間點與該前一單位時間點之相關於該周遭環境的點雲資料，以分別作為該等第一感測資料；及在步驟(B)中，對於每一單位時間點所獲得的點雲資料，藉由該物件分類單元將該點雲資料進行點雲分群以獲得多個不同的點雲群組，並將每一點雲群組標記為一物件，以獲得該等物件。
如請求項2所述的同步定位與建圖優化方法，其中，在步驟(F)中，該同步定位與建圖單元所感測到之相關於該周遭環境的第二感測資料即為該光達模組在該當前單位時間點所獲得的點雲資料，該同步定位與建圖單元係直接濾除該第二感測資料中對應於每一動態物件的點雲群組。
如請求項1所述的同步定位與建圖優化方法，其中，在步驟(C)中，該物件分類單元係篩選出對應尺寸位於一預設範圍內的該等待追蹤物件。
如請求項2所述的同步定位與建圖優化方法，其中，步驟(D)包含以下子步驟： (D-1) 對於每一待比對物件，藉由該物件分類單元根據該待比對物件所對應的點雲群組，獲得該待比對物件之一比對位置； (D-2)對於每一待追蹤物件，藉由該物件分類單元根據該待追蹤物件所對應的點雲群組，獲得該待追蹤物件之一當前位置； (D-3) 對於每一待追蹤物件，根據該待追蹤物件之該當前位置、相關於該待定位物件在該當前單位時間點之移動的一位置與一指向角度，及相關於該待定位物件在該前一單位時間點之移動的一位置與一指向角度，估測該待追蹤物件在該前一單位時間點之前一位置；及 (D-4) 對於每一待追蹤物件，根據該待追蹤物件的該前一位置，及對應該待追蹤物件之待比對物件的該比對位置，將該待追蹤物件分類為該動態物件或該靜態物件。
如請求項2所述的同步定位與建圖優化方法，其中，在步驟(A)中，每一單位時間點所獲得的點雲資料係該光達模組在一自一第一個掃描時間點至一第N個掃描時間點的預設期間內所掃描到的點雲資料，在步驟(B)之前，還包含以下步驟： (H)對於每一單位時間點所獲得的點雲資料，根據相關於該待定位物件自該第一個掃描時間點至該第N個掃描時間點之每一時間點之移動的一位置與一指向角度，校正該點雲資料中在一第二個掃描時間點至該第N個掃描時間點所掃描到的點雲資料，以獲得校正後的點雲資料；其中，在步驟(B)中，每一單位時間點所獲得的點雲資料係包含該第一個掃描時間點所掃描到的點雲資料及經步驟(H)之校正後的點雲資料。
如請求項2所述的同步定位與建圖優化方法，該同步定位與建圖單元包括一用於拍攝該待定位物件之該周遭環境，以獲得相關於該待定位物件之該周遭環境之該第二感測資料的影像拍攝模組，在步驟(E)之前，還包含以下步驟： (I)對於每一被分類為動態物件之待追蹤物件，藉由該物件分類單元根據該待追蹤物件所對應的點雲群組，獲得該待追蹤物件的一物件中心座標；及 (J)對於每一被分類為動態物件之待追蹤物件，根據相關於該點雲資料之一點雲座標系與該第二感測資料之像素座標系之座標系轉換的一座標轉換參數組，將該待追蹤物件的該物件中心座標轉換為一對應於該第二感測資料之像素座標系的另一物件中心座標；其中，在步驟(E)中，該物件分類單元所傳送的分類結果包含每一被分類為動態物件之待追蹤物件的另一物件中心座標。