TWI747195B

TWI747195B - 載具即時精準位置校正系統

Info

Publication number: TWI747195B
Application number: TW109108056A
Authority: TW
Inventors: 李綱; 張瀚元; 陳俊翰; 曾俊為
Original assignee: 國立臺灣大學
Priority date: 2020-03-11
Filing date: 2020-03-11
Publication date: 2021-11-21
Also published as: TW202134151A

Abstract

一種載具即時精準位置校正系統，包含：一慣性感測裝置、一視覺感測裝置、一目標感測裝置及一位置估測裝置，該位置估測裝置藉由估測器模型運算模組及卡爾曼濾波器而對於該慣性感測裝置所取得的載具姿態資訊、該視覺感測裝置所取得的相對姿態資訊、及該目標感測裝置所取得的目標姿態資訊進行資料融合及雜訊過濾，而得出關於待對接目標之即時位置估測資訊。

Description

載具即時精準位置校正系統

本發明相關於一種載具位置校正系統，特別是相關於一種載具即時精準位置校正系統。

隨著工業的發展，自動引導車（Automated Guided Vehicle；AGV）漸漸地被自主移動機器人（Autonomous Mobile Robot；AMR）所取代。兩者最大的差異在於前者需要軌道、磁條和反光條等物件來幫助載具定位及導航；後者則是利用雷射掃描器（Laser Rangefinder）以及相機等感測器對於該環境現有之特徵來進行自主導航。

AGV與AMR常用於自動化工廠中的自主搬運載具，搬運的目標通常為托盤或者是客製化的搬運器材。為了讓機器人能更有效率的運行，自動充電也是不可或缺的功能，在搬運以及自動充電的任務中，精準的估測對接目標與載具的相對姿態扮演著非常重要的角色。

這類自主搬運載具通常配置有多個雷射掃描器，在對接策略上則是預先標示待對接目標至地圖上，依靠高精準度的全域定位系統引領載具至指定位置。此種方式的限制為待對接目標的實際位置與地圖之間幾乎不允許誤差，否則將造成無法正確對接，並在運行的過程中發生危險，但感測器之類的誤差在載具於運動狀態下乃是無可避免，故尚難以作為實際應用的手段。因此，如何讓載具能夠正確完成對接任務是本技術領域的一大課題。

因此，本發明的目的即在提供一種載具即時精準位置校正系統，可提供即時精準位置校正，以讓載具能夠正確且有效地完成對接任務。

本發明為解決習知技術之問題所採用之技術手段係提供一種載具即時精準位置校正系統，係對於正在與一待對接目標進行對接任務的一載具提供即時精準位置校正，該載具即時精準位置校正系統包含：一慣性感測裝置，設置於該載具，該慣性感測裝置即時慣性感測該載具的一加速度而取得關於該載具之載具姿態資訊；一視覺感測裝置，設置於該載具，該視覺感測裝置即時視覺感測一環境影像而取得關於該慣性感測裝置與該視覺感測裝置之間的相對姿態資訊；一目標感測裝置，設置於該載具，該目標感測裝置以即時感測的方式取得關於該待對接目標之目標姿態資訊；以及一位置估測裝置，訊號連接於該慣性感測裝置、該視覺感測裝置及該目標感測裝置，該位置估測裝置具有一估測器模型運算模組及一卡爾曼濾波器，該位置估測裝置藉由該估測器模型運算模組及該卡爾曼濾波器而對於即時取得的該載具姿態資訊、該相對姿態資訊及該目標姿態資訊進行資料融合及雜訊過濾，而得出關於該待對接目標之即時位置估測資訊，並將該即時位置估測資訊提供至該載具之一控制系統。

在本發明的一實施例中係提供一種載具即時精準位置校正系統，其中該慣性感測裝置所即時慣性感測取得的該載具姿態資訊係為關於該載具之六維姿態資訊。

在本發明的一實施例中係提供一種載具即時精準位置校正系統，其中該目標感測裝置係為一視覺感測器、一紅外線感測器或一超音波感測器。

在本發明的一實施例中係提供一種載具即時精準位置校正系統，其中該視覺感測裝置與該目標感測裝置係整合為單一個視覺感測器，而由該視覺感測器即時視覺感測取得該相對姿態資訊及該目標姿態資訊。

在本發明的一實施例中係提供一種載具即時精準位置校正系統，其中該目標感測裝置所即時感測取得的該目標姿態資訊係為關於該待對接目標之六維姿態資訊。

在本發明的一實施例中係提供一種載具即時精準位置校正系統，其中該位置估測裝置中，該卡爾曼濾波器係為一擴展型卡爾曼濾波器。

在本發明的一實施例中係提供一種載具即時精準位置校正系統，其中該位置估測裝置所得出的該即時位置估測資訊為該待對接目標之六維姿態估測資訊。

經由本發明所採用之技術手段，本發明的載具即時精準位置校正系統採用結合有慣性感測裝置及視覺感測裝置的視覺慣性里程計（visual inertial odometry；VIO）的技術作為基礎的定位方式，而非使用習知技術所採用的即時定位與地圖構建（simultaneous localization and mapping；SLAM）技術，並且在此基礎上，以局部端的相對定位取代習知技術的全域定位，再配合以卡爾曼濾波器為基底的演算法，藉此能夠降低運算複雜度，有效達到實時運算，並從而精準地估測待對接目標的姿態。

以下根據第1圖至第5圖，而說明本發明的實施方式。該說明並非為限制本發明的實施方式，而為本發明之實施例的一種。

如第1圖及第2圖所示，依據本發明的一實施例的一種載具即時精準位置校正系統100，係對於正在與一待對接目標O進行對接任務的一載具V提供即時精準位置校正，該載具即時精準位置校正系統100包含：一慣性感測裝置1，設置於該載具V，該慣性感測裝置1即時慣性感測該載具V的一加速度而取得關於該載具V之載具姿態資訊I1；一視覺感測裝置2，設置於該載具V，該視覺感測裝置2即時視覺感測一環境影像而取得關於該慣性感測裝置1與該視覺感測裝置2之間的相對姿態資訊I2；一目標感測裝置3，設置於該載具V，該目標感測裝置3以即時感測的方式取得關於該待對接目標O之目標姿態資訊I3；以及一位置估測裝置4，訊號連接於該慣性感測裝置1、該視覺感測裝置2及該目標感測裝置3，該位置估測裝置4具有一估測器模型運算模組41及一卡爾曼濾波器42，該位置估測裝置4藉由該估測器模型運算模組41及該卡爾曼濾波器42而對於即時取得的該載具姿態資訊I1、該相對姿態資訊I2及該目標姿態資訊I3進行資料融合及雜訊過濾，而得出關於該待對接目標O之即時位置估測資訊I4，並將該即時位置估測資訊I4提供至該載具V之一控制系統C。

具體而言，如第1圖所示，在本實施例中，該載具V係為一堆高機，該待對接目標O為一托盤，該載具V所正在進行的對接任務便是使該堆高機與該托盤正確地對接，以進行物料的搬運。此種對接任務雖然以人工作業方式相當容易完成，但是對於缺少人工輔助的智慧型載具而言卻十分困難。其原因在於，習知的智慧型載具須依靠精準的地圖資訊而引領至正確位置進行對接，而習知的全域定位系統難以提供能夠在局部區域中仍擁有極小誤差的地圖資訊，故容易造成堆高機插針無法完全對接至托盤，並在運行的過程中發生危險。

本發明的載具即時精準位置校正系統100便是針對此種在局部區域中需要精準位置校正的對接任務，藉由提供該待對接目標O相對於該載具V的該即時位置估測資訊I4，而幫助堆高機正確地與該托盤對接。當然，本發明的載具即時精準位置校正系統100所應用的對接任務並不以堆高機與托盤之間的對接為限，諸如智慧型載具與充電座對接而進行充電，也適合應用本發明的載具即時精準位置校正系統100來完成對接。

如第1圖及第2圖所示，依據本發明的一實施例的該載具即時精準位置校正系統100，其中該慣性感測裝置1所即時慣性感測取得的該載具姿態資訊I1係為關於該載具V之六維姿態資訊。具體而言，在本實施例中，該慣性感測裝置1係為用於測量物體三軸姿態角（或角速率）以及加速度的裝置，即，慣性測量單元（Inertial measurement unit；IMU）。該慣性測量單元一般包括三軸的陀螺儀和三個方向的加速度計，來測量物體在三維空間中的角速度和加速度，並以此解算出物體的姿態，其中，該慣性測量單元所解算出的姿態並不是物體在世界座標系中的絕對位置，而是以慣性座標系來看的相對姿態。在本實施例中，該載具姿態資訊I1由於含有根據該載具V在三維空間中的角速度（三個維度）和加速度（三個維度）所解算出的姿態資訊，合計為六個維度，所以是六維姿態資訊。當然，本發明並不限於此，隨著對接任務對於精準度的要求不同，在對於精準度的需求較低的情況下，該載具姿態資訊I1也可以減少至只使用三個維度的資訊，藉此可以使後續運算時的複雜度降低，減少運算所需時間，以更為即時地得到該即時位置估測資訊I4。

如第1圖及第2圖所示，依據本發明的一實施例的該載具即時精準位置校正系統100，其中該視覺感測裝置2與該目標感測裝置3係整合為單一個視覺感測器5，而由該視覺感測器5即時視覺感測取得該相對姿態資訊I2及該目標姿態資訊I3。具體而言，該視覺感測裝置2用於取得關於該慣性感測裝置1與該視覺感測裝置2之間的該相對姿態資訊I2，以藉由結合有該慣性感測裝置1及該視覺感測裝置2的視覺慣性里程計（visual inertial odometry；VIO）的方式，根據該載具姿態資訊I1及該相對姿態資訊I2而估測該載具V的姿態。另一方面，該目標感測裝置3用於取得該待對接目標O之目標姿態資訊I3，以藉由所取得的該目標姿態資訊I3以及所估測的該載具V的姿態而估測出關於該待對接目標O之即時位置估測資訊I4。由於該目標姿態資訊I3並非使用在視覺慣性里程計的方式中，而不限於為視覺影像的資訊，故該目標感測裝置3除了為一視覺感測器之外，亦可以為一紅外線感測器、一超音波感測器或其它類型的感測器。在本實施例中，則是為了簡化硬體、降低設置成本，故該目標感測裝置3係與該視覺感測裝置2同樣是使用視覺感測器，並且與該視覺感測裝置2整合為同一個視覺感測器5。如此一來，從該視覺感測器5所視覺感測的影像中，可同時得到該相對姿態資訊I2及該目標姿態資訊I3。

如第1圖及第2圖所示，依據本發明的一實施例的該載具即時精準位置校正系統100，其中該目標感測裝置3所即時感測取得的該目標姿態資訊I3係為關於該待對接目標O之六維姿態資訊。具體而言，在本實施例中，由於該慣性感測裝置1所即時慣性感測取得的該載具姿態資訊I1係為六維姿態資訊，故為了後續的資料融合，該目標感測裝置3所即時感測取得的該目標姿態資訊I3會與該載具姿態資訊I1同樣為六維姿態資訊。換言之，當該慣性感測裝置1所即時慣性感測取得的該載具姿態資訊I1改採用三維姿態資訊時，該目標感測裝置3所即時感測取得的該目標姿態資訊I3同樣為三維姿態資訊即可。

如第2圖所示，依據本發明的一實施例的該載具即時精準位置校正系統100，其中該位置估測裝置4中，該卡爾曼濾波器42係為一擴展型卡爾曼濾波器。卡爾曼濾波器（Kalman filter）是一種能夠使用含有雜訊的量測資訊而估測動態系統的狀態的濾波器，擴展型卡爾曼濾波器（extended Kalman filter；EKF）則進一步適用於非線性系統。如第1圖所示，在本發明中，該視覺感測裝置2及該目標感測裝置3並非是處在靜止狀態下而自外部來對該對接任務的系統進行感測，而是設置於正在與該待對接目標O進行對接任務的該載具V（堆高機），且在本實施例中，該視覺感測裝置2及該目標感測裝置3係進一步係設置在該堆高機的貨叉上，而會隨著堆高機移動且隨著該貨叉升降，因此感測結果充滿雜訊，所得到的姿態資訊中含有大量的姿態誤差。這樣的姿態誤差若是在一般的用於道路行駛的車輛導航系統中，可能因為相對於系統而言的誤差範圍太小而可以忽視。然而，對於本發明所針對的局部區域的對接任務而言，這樣的姿態誤差會造成不容忽視的影響，故在本實施例中藉由該卡爾曼濾波器42來排除誤差，以精準地得出該待對接目標O之即時位置估測資訊I4。另外，該卡爾曼濾波器42的基本架構大致如第3圖所示，其係基於初始狀態（initial state）及之前狀態（previous state），經過狀態傳播（propagate state）及計算量測雜訊共變異數（measurement noise covariance）及卡爾曼增益（Kalman gain），同時計算量測殘差（measurement residual）而進行狀態更新。

如第1圖及第2圖所示，依據本發明的一實施例的該載具即時精準位置校正系統100，其中該位置估測裝置4所得出的該即時位置估測資訊I4為該待對接目標O之六維姿態估測資訊。具體而言，在本實施例中，由於該慣性感測裝置1所即時慣性感測取得的該載具姿態資訊I1以及該目標感測裝置3所即時感測取得的該目標姿態資訊I3皆為六維姿態資訊，故該位置估測裝置4所得出的該即時位置估測資訊I4同樣也會是六維姿態估測資訊。

請參閱第4圖，並配合第1圖及第2圖，以下將說明本發明的實施例的載具即時精準位置校正系統100的運作流程。在本實施例中，該載具即時精準位置校正系統100在該視覺感測器5透過視覺感測取得影像資料後，則利用演算法做特徵點抓取與比對（步驟S10）。具體而言，在本實施例中，是基於如第5圖所示的機器視覺的幾何關係而抓取出特徵點。第5圖所顯示的是一種被稱為三焦張量（Trifocal tensor）的機器視覺的幾何關係，特別是其中的「point-line-point」的幾何模型。簡單而言，在該幾何模型中，隨著該視覺感測器5的移動，線段上的點P在不同影像上投影為點P1及點P2，點P1與點P2之間有單應性（Homography）的關係，故透過單應性矩陣便能夠自點P的先前的位置（點P1）求出之後的位置（點P2）。當然，所採用的幾何模型並不以此為限，依據情況的不同，亦可以採用「point-point-point」、「line-line-line」等的幾何模型。

另一方面，在該視覺感測器5視覺感測取得影像資料的同時，該慣性感測裝置1感測三軸角速度與三軸加速度，用於狀態傳播（步驟S20），而該視覺感測器5的影像資料則用於量測更新（步驟S30）。藉此，利用該慣性感測裝置1的動態模型並搭配量測模型得到的結果來估測得到該待對接目標O之六維姿態（即時位置估測資訊I4）（步驟S40）。

藉由上述結構，本發明的載具即時精準位置校正系統100採用結合有該慣性感測裝置1及該視覺感測裝置2的視覺慣性里程計（visual inertial odometry；VIO）的技術作為基礎的定位方式，而非使用習知技術所採用的即時定位與地圖構建（simultaneous localization and mapping；SLAM）技術，並且在此基礎上，以局部端的相對定位取代習知技術的全域定位，再配合以卡爾曼濾波器42為基底的演算法，藉此能夠降低運算複雜度，有效達到實時運算，並從而精準地估測該待對接目標O的姿態。

以上之敘述以及說明僅為本發明之較佳實施例之說明，對於此項技術具有通常知識者當可依據以下所界定申請專利範圍以及上述之說明而作其他之修改，惟此些修改仍應是為本發明之發明精神而在本發明之權利範圍中。

100:載具即時精準位置校正系統 1:慣性感測裝置 2:視覺感測裝置 3:目標感測裝置 4:位置估測裝置 41:估測器模型運算模組 42:卡爾曼濾波器 5:視覺感測器 C:控制系統 I1:載具姿態資訊 I2:相對姿態資訊 I3:目標姿態資訊 I4:即時位置估測資訊 O:待對接目標 P:點 P1:點 P2:點 S10:步驟 S20:步驟 S30:步驟 S40:步驟 V:載具

［第1圖］為顯示根據本發明的一實施例的載具即時精準位置校正系統的示意圖；［第2圖］為顯示根據本發明的實施例的載具即時精準位置校正系統的方塊示意圖；［第3圖］為顯示根據本發明的實施例的載具即時精準位置校正系統的卡爾曼濾波器的架構示意圖；［第4圖］為顯示根據本發明的實施例的載具即時精準位置校正系統的運作流程示意圖；［第5圖］為顯示根據本發明的實施例的載具即時精準位置校正系統的所應用的視覺感測幾何模型示意圖。

100:載具即時精準位置校正系統

1:慣性感測裝置

2:視覺感測裝置

3:目標感測裝置

4:位置估測裝置

41:估測器模型運算模組

42:卡爾曼濾波器

5:視覺感測器

C:控制系統

I1:載具姿態資訊

I2:相對姿態資訊

I3:目標姿態資訊

I4:即時位置估測資訊

O:待對接目標

V:載具

Claims

一種載具即時精準位置校正系統，係對於正在與一待對接目標進行對接任務的一載具提供即時精準位置校正，該載具即時精準位置校正系統包含：一慣性感測裝置，設置於該載具，該慣性感測裝置即時慣性感測該載具的一加速度而取得關於該載具之載具姿態資訊，該載具姿態資訊係為關於該載具之六維姿態資訊；一視覺感測裝置，設置於該載具，該視覺感測裝置即時視覺感測一環境影像而取得關於該慣性感測裝置與該視覺感測裝置之間的相對姿態資訊；一目標感測裝置，設置於該載具，該目標感測裝置以即時感測的方式取得關於該待對接目標之目標姿態資訊，該目標姿態資訊係為關於該待對接目標之六維姿態資訊；以及一位置估測裝置，訊號連接於該慣性感測裝置、該視覺感測裝置及該目標感測裝置，該位置估測裝置具有一估測器模型運算模組及一卡爾曼濾波器，該位置估測裝置藉由該估測器模型運算模組及該卡爾曼濾波器而對於即時取得的該載具姿態資訊、該相對姿態資訊及該目標姿態資訊進行資料融合及雜訊過濾，而以該待對接目標與該載具之間的局部端相對定位方式得出關於該待對接目標相對於該載具而相對定位之即時位置估測資訊，該即時位置估測資訊為該待對接目標之六維姿態估測資訊，並將該即時位置估測資訊提供至該載具之一控制系統。
如請求項1所述之載具即時精準位置校正系統，其中該目標感測裝置係為一視覺感測器、一紅外線感測器或一超音波感測器。
如請求項1所述之載具即時精準位置校正系統，其中該視覺感測裝置與該目標感測裝置係整合為單一個視覺感測器，而由該視覺感測器即時視覺感測取得該相對姿態資訊及該目標姿態資訊。
如請求項1所述之載具即時精準位置校正系統，其中該位置估測裝置中，該卡爾曼濾波器係為一擴展型卡爾曼濾波器。