同步定位與地圖構建於無人車軌跡跟蹤控制

同步定位與地圖構建於無人車軌跡跟蹤控制

  • 謝至亮 et al.
  • 11865

 

※本文原刊登於財團法人精密機械研究發展中心出版之《技術通報》第275期, p. 21-29, 2020.04.15.，經該刊物授權轉載※

謝至亮、黃自誠、曾文詮、張嘉熒、陳顯禎

摘要

欲使無人地面載具(unmanned ground vehicle，UGV)於複雜的環境中進行任務工作，除了車體本的機電系統外，需要掌握精準定位與跟蹤控制，本文介紹同步定位與地圖構建技術(simultaneous localization and mapping，SLAM)，主以RTAB-Map為基礎加以強化，針對深度相機(RGB-D camera)純視覺里程計(vision odometry)的缺點，加入慣性量測單元(inertial measurement unit)與GPS-RTK的多感知器關鍵技術融合(sensor fusion)，使受干擾情況下的靜態定位誤差由 ±5.2 mm降低至 ±3 mm；於軌跡跟蹤的問題上，藉由模型預測控制(model predictive control)方式，可在具備上述高精度SLAM定位的情況下，使UGV更能應付場域中崎嶇不平且多變的環境精準行走。

一、簡介

近年來無人地面載具(unmanned ground vehicle，UGV)是一大熱門話題，然早在西元1920年開始即有相關研究的產出，1980年第一台自動駕駛車出現，但技術還未成熟。直到21世紀，伴隨著軟硬體的進步，google的全自動駕駛車已於2012年美國內華達州開始上路測試，且特斯拉汽車近年率先推出自動駕駛汽車搶市。在自駕車的領域中包含高精度地圖、動態避障、車輛定位、車體控制等關鍵技術，研究範圍涵蓋了很多的課題，主要的動機也就是為了就是取代重覆性的行為。

對於要在複雜環境中進行任務工作的任務型UGV而言，位置姿態的精準定位及軌跡跟蹤的UGV控制是兩個相當重要問題，像是在農業場域、災害場域等等複雜且時變的環境中行駛時，並無法如同在道路環境中，使用事先建立的高精度全局環境數據地圖搭配固定的道路特徵來精準定位出UGV的位置與姿態；崎嶇不平、路面性質多變等環境因素，亦會造成UGV的控制不容易達成軌跡跟蹤。為了克服以上難題，本文的研究是使用深度相機來進行同步定位與地圖構建(simultaneous localization and mapping，SLAM)，藉由RTAB-Map在畫面中特徵點的抓取與比對分析，計算出相機的位置與姿態，再藉此反算來定位出UGV所在的位置與姿態，並同時將環境的三維(three-dimensional，3D)地圖建構出來，如此提供視覺里程計(vision odometry，VO)。但因相機會因環境變化過大時或是特徵點比對失敗時而無法有效定位出所需要的資訊，因此加入了慣性感測單元(inertial measurement unit，IMU)或稱為慣性里程計(inertial odometry，IO)的感測融合(sensor fusion)，成為視覺慣性里程計(visual-inertial odometry，VIO)，使位置與姿態的取得更加強健，不易受干擾影響。除了透過VIO取得強健、高精度的相對位置與車體姿態，亦使用GPS-RTK技術，獲得高精度的絕對位置，並將各項感知器的資訊進行融合來強健里程計的功能。

此外UGV實際行走時，會因環境中的變因而使得UGV無法走到預計的速度或是位置，使得UGV無法達成軌跡跟蹤，因此本文介紹模型預測控制(model predictive control，MPC)的理論，當前述同步定位與地圖構建能提供即時的精準位置姿態後，模型預測控制會對未來一序列時刻的系統狀態進行預測，求解一串最優控制序列，並能於其中加入約束條件，因此能更好的應付環境變因，在場域中崎嶇不平且性質多變的地面上，使UGV每一步的行走能精準到位，達到更精確的軌跡跟蹤控制。然而，模型預測控制理論相較傳統控制理論有較大的計算量，因此需要耗費較多運算資源，若要將其實際應用於複雜環境中的任務型UGV上，尚需進行進一步研究與開發。

二、即時定位地圖構建(Simultaneous Localization and Mapping)

同時定位與地圖構建(SLAM)通常是指利用各種傳感器數據(光達、RGBD相機、IMU等等)生成對其自身位置姿態的定位和場景地圖信息的演算法。當前主流的演算法主要分為基於光達(LiDAR)的光達SLAM和基於視覺的vSLAM。其中光達SLAM，主要使用光達進行測距，優點是穩定、地圖資訊簡單且準確、誤差模型簡單、計算量相對vSLAM較少，目前已經廣泛應用在業界上。缺點則有感測器資訊量較少導致累積誤差的消除相較vSLAM更不容易，且光達本身價格高且體積大，使用上不如vSLAM方便。此外，光達SLAM其實不擅長於動態環境中工作，在許多行人行走的室內環境會嚴重影響SLAM的定位功能。而vSLAM，第一篇論文於2006年公開，但近幾年相當熱門[1]。相較於光達SLAM，vSLAM更貼近於人類獲取環境資訊的方式，具有強大的環境辨識能力，但也因此運算量龐大，導致實時運行有難度。然而近幾年晶片的計算性能的提升，也造就了這門技術再度熱門起來。雖然計算量是致命的缺點，但在現今已不是大問題；而且vSLAM具有感測器價格便宜、體積較小且安裝方便的優點，這也是近年熱門的主因。當前主流的演算法有ORB-SLAM、LSD-SLAM、RTAB-Map等等。這些演算法的差異主要在於對於特徵點的提取以及地圖稠密度，理想特徵點必須具有可重複性、可區別性、高效率和本地性(locality)；為了做到可重複性，期許特徵點具有尺度、旋轉、灰度不變性。滿足這些特性犧牲的往往是計算時間，計算時間與特徵點提取方式必須取得一個平衡。接下來介紹於此技術中主要的三個模塊：特徵點擷取ヽ建圖及迴環檢測。

A. 特徵點擷取(Feature Extraction)

根據影像變化估計相機的行徑運動狀態即為視覺里程計，而如何估算影像的變化所使用的技術即為特徵點比對。特徵點在影像中就像是路標，我們期許這些點在相機移動的過程中能保持穩定，當相機移動的過程不論是發生相機旋轉、前進後退會者外在環境光影變化都會希望這些特徵點的特性還是不變；著名的特徵點擷取有：尺度不變特徵轉換(scale-invariant feature transform)、加速強健特徵(speeded up robust features)、ORB特徵檢測算法(oriented FAST and rotated BRIEF，ORB)等等，這些特徵點都具有以下特性：可重複性、可區別性、高效率性和本地性，且特徵點分為兩個部分：關鍵點(key-point)與描述子(descriptor)。關鍵點為影像中較穩定、不會隨著相機旋轉而丟失的點，典型的算法有：Harris、FAST (features from accelerated segment test)等等；描述子則賦予了特徵點可區別性，使得在處理影像時具有區分關鍵點的能力，典型算法有BRIEF (binary robust independent elementary features)。

B. 建圖(Mapping)

地圖的建構是將所有取得的特徵點也就是路標，一個個依其定位與對應關係建構出一個完整包含所有路標的地圖，而以地圖所設定的應用與取法不同，可分為稀疏地圖、稠密地圖與語義地圖。稀疏地圖即為只有特徵點資訊；稠密地圖則是路過的所有看過的資料都建成地圖；而語義地圖即是定義了地圖中物件的名稱，如桌子，椅子等等。

使用的vSLAM技術為RTAB-Map，此方法使用深度相機，並將深度的計算硬體化的節省計算時間，並以基於特徵的VO與詞袋模型的迴環檢測，再經由後端的位姿優化再將資訊傳送到UGV，主要流程如圖3(a)所示為vSLAM的基本流程圖，而圖3(b)即為我們使用的系統，以RTAB-Map為基礎改良的流程圖。接下來我們會針對可改善的與加強的部份再加以討論：以本體上再加上一個感測試進行融合的VIO與導入外部公正第三者的資訊提供正確的世界座標的GPS+RTK，並且應變於場地不可控環境而加入MPC。

圖三(a)、vSLAM流程圖[2]。


圖三(b)、VIO與MPC於UGV導航流程圖。


C. 迴環檢測(Loop Closure Detection)

由於特徵比對後建構出來的地圖會隨著定位過程中的不確定性而慢慢產生誤差，特別是姿態旋轉的累積誤差更為嚴重。為了消除這些累積誤差，2006年提出迴環檢測這個想法[3]，當車子回到相同的環境時就對全局地圖進行一次修正，然而每次相機移動位置補取關鍵幀後都要對全局地圖的每一張關鍵幀進行匹配檢驗，這太花時間了，於是產生如何先得知哪裡可能出現迴路進行修正的想法，就這樣延伸出兩個做法：第一個是透過里程計來得知相機移動到了某個位置附近，所以匹配過程只利用相機位置附近的影像進行匹配就好，然而這個方法實作上並不可行，因為這樣的想法必須建立在已知「相機回到之前某個位置」這一事實，然而誤差的產生本身就是隨機的，我們就是不確定相機位置到底飄到了哪裡才需要回環檢測，所以這個方法有點倒因為果；另一個方法是詞袋模型(bag-of-words，BoW)，方法是記錄每一張影像的特徵點個數得到一個像是字典的東西，然後再透過當前影像的特徵點個數去搜尋有可能匹配的影像，找到有可能匹配的影像後再去檢查匹配影像的特徵點是否有在當前畫面中出現，這樣的作法可以大幅降低回環檢測過程中的計算量。

三、視覺慣性里程計(Vision Inertial Odometry)

視覺里程計(VO)是由D. Nister等在2004年路標論文所提出[4]，是一種利用連續影像序列來估計機器人移動距離和方向的方法。在經過許多研究團隊的努力開發後，近幾年VO在剛體、亮暗不變、靜態且特徵點足夠的環境中已經能穩定定位，但遇到環境色調單一特徵點不足(像白牆、玻璃等等)或影像快速變動(像行人在鏡頭前面走來走去、車體本身的受到突如其來外力產生的震盪、粗糙的路面…等等)時，定位的誤差就會越來越大，導致建構的地圖越來越不精確[5]。為了解決此問題，許多研究人員開始加入IMU來進行補償。IMU能夠測量行進間的角速度和加速度，短時間內具有高定位精度，所以當視覺感測器兩幀之間特徵點數目不足或影像快速變動導致定位不精確時可以參考IMU的數據進行補償。但IMU也有缺點，IMU提供的資訊隨著長時間下來會產生累積誤差，此時可以利用影像定位來進行消除。因此從結論來看相機感測器與IMU是具有明顯的互補性，從實驗的結果也可以發現VIO確實提供一個比較穩定的位姿(旋轉加平移)資訊。

而融合VO與IO的方法有很多，[6]中比較了許多方法與各自的缺點，當中基於優化(optimization-based)的方法可實現出色的定位精度並降低內存利用率，而基於過濾(filtering-based)的方法在計算資源方面具有優勢，典型的融合算法有擴展卡爾曼濾波器( extended Kalman filter，EKF) [7]、無損濾波器(unscented Kalman filter，UKF) [8]與多狀態約束卡爾曼濾波器(multi-state constraint Kalman filter，MSCKF) [9]測試其穩定度。在我們的系統中，為了因應計算即性的而選擇使用了UKF進行融合。圖4是融合後的結果，圖4(a)與4(b)是靜態無干擾源的定位結果，圖4(c)與4(d)是靜態有干擾源(人在相機感測器前來回走動)的定位結果，圖中淡紫色圓球是一個半徑5.5 mm大小的圓球。從圖4(a)與4(b)中可以發現VO的誤差值在±3.2 mm以內，VIO誤差縮小至±1.8 mm內；而圖4(c)與4(d)中有干擾源VO的誤差值在±5.2 mm以內，VIO更將誤差縮小±3.0 mm以內；系統於靜止時不論有無干擾源的情況下VIO的定位精度都是優於VO的定位精度。


四、即時動態定位技術與GPS

即時動態定位技術(real-time kinematic，RTK)是一種用來提升衛星導航系統(global navigation satellite systems，GNSS)精準度的技術，可將原本公尺等級的精準度提升到公分等級，若要瞭解RTK提升精準度的原理，則需要先了解GNSS的原理。目前世界上的GNSS系統包含美國的GPS系統、俄羅斯的GLONASS系統、歐盟的Galileo系統、中國北斗(BeiDou)系統、日本的QZSS系統、印度的IRNSS系統，其定位原理首先需要計算出地面接收端與太空中衛星之間的距離，再藉由與四顆以上衛星的距離組成聯立方程組， 計算出待測物在3D空間中的位置，而RTK所扮演的角色，在於提升「地面接收端與太空中衛星之間的距離」的精準度。

GNSS獲得距離的最基本原理[10]為：衛星向地面接收端發射電磁波訊號，地面接收端與太空中衛星之間的距離=電磁波傳遞的時間×電磁波的速率。由於電磁波以光速傳遞，時間上1 μs的誤差就會對距離造成大約300 m的誤差，因此關鍵技術在於如何精準的量測出傳遞時間。以GPS系統的參數為例，太空中的衛星向地面發射電磁波訊號，訊號選用不易受大氣吸收的1575.42 MHz (L1頻段)作為載波(carrier)的頻率，訊號中包含了碼元速率1.023 Mcps (chips per second)*1的C/A碼(coarse/acquisition code)，和位元速率50 bps (bits per second)的導航訊息(navigation message)。導航訊息包含了該訊號被發射的時間點，衛星在太空中的位置等等要告知接收端的資料；C/A碼則是讓接收端用來測量傳遞時間的偽隨機雜訊(pseudorandom noise，PRN)，PRN是一種具有雜訊性質的週期性序列，每顆衛星的PRN都是獨特且公開給民間知道的*2，不同PRN之間幾乎沒有互相關(cross-correlation)，而PRN自己除了在零延遲外，幾乎沒有自相關(autocorrelation)，因此地面接收端只需要將收到的PRN與公開的各顆衛星PRN做相關運算，做完相關後有極值的，表示訊號是來自那一顆衛星，極值在延遲軸上的位置則表示電磁波傳遞的時間。

由於用來測距(傳遞時間)的C/A碼是1.023 Mcps，代表其每一個碼元(chip)在空間中的長度(波長)約為300 m，做相關運算時，就算對於C/A碼相位的精準度是波長的1%，測距的精準度也只達到3 m。在RTK技術中使用的不是C/A碼相位，而是載波的相位，由於載波頻率是1575.42 MHz，波長大約只有19 cm，因此能將測距的精準度提升至公分等級。然而，載波並不像C/A碼的PRN為已知的序列，因此在RTK技術中，必須同時使用基準站(base)和移動站(rover)，利用基準站已知的座標，搭配基準站和移動站彼此之間的相位差，來反算求得移動站位置，此種差分做法可以順便消除大氣擾動對於電磁波速率造成的影響。利用上述載波相位差分的技術，RTK能在GNSS的基礎上，進一步解算出公分等級的定位結果。進一步實測，圖5為靜態測試RTK與GNSS的誤差實驗，於空曠的頂樓屋頂放置一段時間後開始取樣分析用。圖5(a)的左半邊為我們RTK靜態誤差訊號最小的圓圈半徑為2.5 cm，圖5(b)的左半邊GNSS靜態誤差訊號最小的圓圈半徑12.5 cm，分別測試他們的定位精度，每個綠色的點現在GPS定到自己的位置，而且分別代表的代表不同座標系，它會以1Hz的頻率不斷增加，RTK的誤差於5 cm內，而GNSS誤差於25 cm內如紅框所示，而右半邊圖表以顏色代表衛星訊號的品質。


五、模型預測控制(Model Predictive Control)

我們MPC控制方式應用在UGV的局部路徑規劃上，車體導航的路徑規劃其實可以劃分為全局與局部路徑規劃，全局路徑規劃的目的是在已知全局地圖的情況下，規劃出一條可行進的理想路線，經典的演算法有A*(A star) [11]和Dijkstra演算法[12]等等。局部路徑規劃則是在已知理想路線時，即時計算出車體的線速度與角速度讓車體對這條理想路徑進行追跡，典型的算法有：純追蹤[13]、模糊神經控制[14]、滑模控制[15]、非線性模型預測控制[16]與強健非線性模型預測控制[17]，當前UGV的局部路徑規劃是使用非線性模型預測控制，接下來我們著重介紹非線性模型預測控制(nonlinear MPC)。

模型預測控制運作的過程可簡單解釋成：在每一個取樣時刻通過當前時刻狀態與系統輸入得出未來時刻系統狀態模型，並利用未來時刻系統狀態模型與優化器求解一串最優控制序列，且得到下一時刻最優控制序列的控制方法。而模型預測控制演算法有三個重要要素：預測模型，滾動優化，反饋校正。

A. 預測模型：預測模型是預測控制的基礎，在取得受控體的系統參數、當前時刻的系統狀態以及輸入後，預測出受控體未來輸出響應與未來輸入變化量之間的對應關係。

B. 滾動優化：預測控制的優化並不是離線進行，而是隨著採樣時刻的前進反覆地在線進行，將預測模型與目標路徑等資訊丟給優化器後，優化器會在每一個採樣時刻都會得出一串最優控制序列，且每一個時刻的都會對當前預估的預差不斷重新滾動計算。

C. 反饋校正：通過優化器計算得出的最優控制序列後，僅使用在下個時刻進行控制，到了下個時刻會重新進行模型預測以及滾動優化，從而得到一組新的最優控制序列，此稱為反饋校正。

MPC最大的優點就是約束的設計，控制問題常常會受限於硬體上的限制而無法得到最佳解，但模型預測控制則可以透過預測模型以及優化器求解不等式約束的線性規劃問題進而取得一組不違背約束的最優控制序列，換個方式簡單的方式說，假設受控體為汽車，而汽車的速限為時速80 km/hr，在得知此約束後透過模型預測控制即可計算出一串不違背此約束的最佳控制序列。但缺點即為不能應用在不穩定的系統，且使用前必須有系統參數，無法及時進行系統辨識。

我們使用Matlab進行模擬實驗，模擬模型預測控制追跡的結果，圖6(a)為車子追跡的路徑，圖6(b)為分別為車體橫向偏移、車體角度偏移及車輪角度偏移，圖6(c)為車體與目標路徑的差異。從圖中可以看出MPC的兩個特色，首先MPC與PID控制器的差異在於MPC並不會只考慮當下的誤差，而是一次性考慮未來預測時域內的誤差，並以此結果透過約束與優化器來計算最佳控制序列。這種感覺其實就像是人在視野良好的情況下開車。人們會依據眼睛捕捉到的資訊在腦海中進行預想的路徑規劃。假設發現要過彎時甚至會提前放慢速度甚至是略略的出彎來讓過彎路徑更平穩一些。從圖6(b)中可以看出車子每次在預期到大轉彎要出現時，都會做一個出彎的動作來使得過彎能更平滑。另一個MPC的特色則是約束，預測模型可以得出未來輸入變化量與預期結果的對應關係。因此可以透過約束來限制導航的過程。約束分為軟約束與硬約束：硬約束一般是車體本身的限制，軟約束則普遍是一些不希望發生的事情，例如急轉彎或者是離目標路徑太遠等等。從圖6(c)中可以看到，最終車子追跡的過程距離目標路徑誤差小於5 cm。


六、結論

本文以RTAB-Map為基礎加以強化UGV的同步定位與地圖構建，利用加入慣性量測單元(IO)形成強健抗干擾的視覺慣性里程計(VIO)，能改善深度相機純視覺里程計因環境變化過大或特徵點比對失敗的缺點，再加上GPS-RTK的精確絕位置獲取，達到多感知器融合，使受干擾情況下的靜態定位誤差由 ±5.2 mm降低至 ±3 mm。本文亦介紹模型預測控制顯示了其應用於複雜環境中的潛力，於程式模擬中，在具備高精度定位的情況下，UGV能在場域中崎嶇不平且性質多變的地面上，達到更精確的軌跡跟蹤控制，然而，模型預測控制理論所需的計算量大，若要將其應用於實際UGV尚需進行進一步研究與開發。


註解

註1 C/A碼的每一個0、1稱之為碼元，與數位訊號中位元的概念相同，但因為其並非資料，所以使用不同名稱。

註2 軍用的測距碼稱為P碼(precision code)，其PRN不公開，資料速率為10.23Mcps (C/A碼的10倍)，一個chip的長度約30 m。

致謝

This work was supported by Ministry of Science and Technology (MOST) of Taiwan (MOST 108-2321-B-009-002 and MOST 109-2321-B-009-004).

參考文獻

  1. N. Karlsson, E. Di Bernardo, J. Ostrowski, L. Goncalves, P. Pirjanian, and M. Munich, “The vSLAM algorithm for robust localization and mapping,” Int. Conf. on Robotics and Automation (ICRA), 24–29 (2005).
  2. 高翔, 視覺SLAM十四講：雙倍內容強化版, 深智數位, 2–10 (2019).
  3. K. L. Ho and P. Newman, “Loop closure detection in SLAM by combining visual and spatial appearance,” Robotics and Autonomous System 54, 740–749 (2006).
  4. D. Nister, O. Naroditsky, and J. Bergen, “Visual odometry,” Proceedings of the 2004 IEEE Computer Society Conference on Computer Vision and Pattern Recognition, I–I (2004).
  5. S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. l. Furgale, “Keyframe-based visual-inertial odometry using nonlinear optimization,” International Journal of Robotics Research 34(3), 314-334 (2015).
  6. C. Chen, H. Zhu, M. Li, and S. You, “A review of visual-inertial simultaneous localization and mapping from filtering-based and optimization-based perspectives,” Robotics 7(3), 45–66 (2018).
  7. M. Bloesch, S. Omari, M. Hutter, and R. Siegwart, “Robust visual inertial odometry using a direct EKF-based approach,” Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, 298–304 (2015).
  8. E. A. Wan and R. Van Der Merwe, “The unscented Kalman filter for nonlinear estimation,” Proceedings of the IEEE 2000 Adaptive Systems for Signal Processing, Communications, and Control Symposium, 153–158 (2000).
  9. A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint Kalman filter for vision-aided inertial navigation,” Proceedings of the IEEE International Conference on Robotics and Automation, 3565–3572 (2007).
  10. P. F. Lammertsma, Satellite Navigation, Institute of Information and Computing Sciences Utrecht University (2005).
  11. P. E. Hart, N. J. Nilsson and B. Raphael, “A formal basis for the Heuristic determination of minimum cost paths,” IEEE Transactions on Systems Science and Cybernetics 4(2), 100–107 (1968).
  12. E. W. Dijkstra, “A note on two problems in connexion with graphs,” Numerische Mathematik 1, 269–271(1959).
  13. M. Bergerman et al., “Robot farmers: Autonomous orchard vehicles help tree fruit production,” IEEE Robotics & Automation Magazine 22(1), 54–63 (2015).
  14. E. Kayacan, E. Kayacan, and M. A. Khanesar, “Identification of nonlinear dynamic systems using type-2 fuzzy neural networks—A novel learning algorithm and a comparative study,” IEEE Transactions on Industrial Electronics 62(3), 1716–1724 (2015).
  15. J. Taghia, X. Wang, S. Lam et al. “A sliding mode controller with a nonlinear disturbance observer for a farm vehicle operating in the presence of wheel slip,” Auton. Robot 41, 71–88 (2017).
  16. R. Lenain, B. Thuilot, C. Cariou et al., “High accuracy path tracking for vehicles in presence of sliding: Application to farm vehicle automatic guidance for agricultural tasks,” Auton. Robot 21, 79–97 (2006).
  17. E. Kayacan, E. Kayacan, H. Ramon, and W. Saeys, “Robust tube-based decentralized nonlinear model predictive control of an autonomous tractor-trailer system,” IEEE/ASME Transactions on Mechatronics 20(1), 447–456 (2015).

作者簡介

謝至亮先生為國立交通大學光電學院碩士，現為瑞士洛桑聯邦理工學院博士生。

黃自誠先生為國立交通大學光電學院碩士生。

曾文詮先生為國立交通大學光電學院碩士生。

張嘉熒小姐為國立成功大學電腦與通訊研究所博士，現為國立交通大學智慧光電農業機器人研究中心助理研究員。

陳顯禎先生為美國加州大學洛杉磯分校機械工程學系博士，現為國立交通大學光電學院教授兼院長。