使用多個狀態限制卡爾曼濾波器與三焦張量幾何之視覺輔助慣性測程器

國 立 交 通 大 學

電控工程研究所

碩 士 論 文

使用多個狀態限制卡爾曼濾波器與三焦張量幾何之

視覺輔助慣性測程器

Visual Assisted IMU Odometer Using Multi-State Constrained

Kalman Filter and Tri-focal Tensor Geometry

研 究 生：陳 鳴 遠

指導教授：胡 竹 生 博士

使用多個狀態限制卡爾曼濾波器與三焦張量幾何之

視覺輔助慣性測程器

Visual Assisted IMU Odometer Using Multi-State Constrained

Kalman Filter and Tri-focal Tensor Geometry

研 究 生：陳 鳴 遠

Student: Ming-Yuan Chen

指導教授：胡 竹 生 博士

Advisor: Dr. Jwu-Sheng Hu

國 立 交 通 大 學

電 控 工 程 研 究 所

碩 士 論 文

A Thesis

Submitted to Institute of Electrical Control Engineering

College of Electrical and Computer Engineering

National Chiao Tung University

in partial Fulfillment of the Requirements

for the Degree of Master

in

Electrical Control Engineering

July 2013

Hsinchu, Taiwan, Republic of China

i

使用多個狀態限制卡爾曼濾波器與三焦張量幾

何之視覺輔助慣性測程器

研究生：陳 鳴 遠

指導教授：胡 竹 生 博士

國立交通大學電控工程研究所碩士班

摘要

本論文提出一套結合單一攝影機與慣性量測裝置的測程器架構。由於慣性量測裝置 存在誤差累積的問題，導致單純使用慣性量測裝置的測程器在真實環境下很難得到準確 的結果，必須要有其他感測器輔助。本論文以單一攝影機與慣性量測裝置進行感測器融 合，由於使用了攝影機的資訊，慣性量測裝置的誤差可以被有效地抑制住，使得慣性量 測裝置可以提供實際地圖尺度。 本論文使用在三張影像中存在的三焦張量幾何關係作為攝影機所提供的量測資訊， 此方法可以不需要估測特徵點在空間中的位置，也就是不需要進行環境的重建。同時本 論文將三張影像分別對應到的攝影機姿態在濾波器中修正，形成一多個狀態限制的卡爾 曼濾波器，以此提出一個在計算量與精確度間取得平衡的滑動視窗式測程法，相較於現 存視覺式測程法或是同時定位與地圖建立的方法，本論文提出的架構更符合自我軌跡估 測的測程需求並且適用於即時導航系統。本論文同時提出基於三視角幾何的隨機抽樣一 致演算法，它可以有效地排除比對錯誤或是落於移動物體上的特徵點，使得整體演算法 在動態的環境下仍有可靠的結果。最後，本論文實作一套整合單一攝影機、慣性量測裝 置以及 GPS 的硬體設備並以在真實環境下的資料驗證所提出的演算法。

ii

Visual Assisted IMU Odometer Using Multi-State

Constrained Kalman Filter and Tri-focal Tensor Geometry

Student：Ming-Yuan Chen

Advisor：Dr. Jwu-Sheng Hu

Institute of Electrical Control Engineering

National Chiao Tung University

Abstract

This thesis presents an odometer architecture which combines a monocular camera and an inertial measurement unit (IMU). Due to error accumulation problem, it is very hard to get reliable result by only using the IMU, therefore there is a need to fuse the information with other sensors. In this dissertation, a monocular camera and an IMU are used for sensor fusion. By using the information of the camera, the error of IMU can be effectively constrained, so that the IMU can provide real scale.

In this dissertation, the trifocal tensor geometry relationship between three images is used as camera measurement information, which makes the proposed method without estimating the 3D position of feature point. In other words, the proposed method does not have to reconstruct environment. Meanwhile, the camera pose corresponding to each of the three images are refined in filter to form a multi-state constraint Kalman filter. Consequently, this dissertation proposes a sliding window odometry which has a balance between

computational cost and accuracy. Compared with traditional visual odometry or simultaneous localization and mapping (SLAM) method, the proposed method not only meets the

requirement of odometer in ego-motion estimation, but also suit for real-time application. This dissertation further proposes a random sample consensus (RANSAC) algorithm which is based on three views geometry. The RANSAC algorithm can effectively reject feature points which are mismatch or located on independently moving objects, thus it make the overall algorithm capable of operating in dynamic environment. Finally, a hardware which integrates with a monocular camera, an IMU and a GPS is implemented, and experiments are conducted to validate the proposed method in real environment.

iii

致謝

本論文的完成，首先感謝指導教授胡竹生老師的教導，在做為專題生兩年和碩士生兩 年的期間中，從老師身上學習到許多電機方面的專業知識，而老師對於做研究的嚴謹態 度和解決問題的不屈不折精神，著實讓我大開眼界，另外也很感謝老師在我碩二上時， 引薦我至工研院打工，在此獻上對老師的最高謝意。 感謝實驗室的學長、同學與學弟，四年的實驗室生涯有了你們才能如此順利。感謝帶 我進入影像領域的永融學長、教導我許多硬體知識的阿吉學長、共同執行國科會計畫的 勁源和建安學長，每次與你們討論的過程總是讓我受益良多，你們所給的建議也總是相 當實用。感謝分享博士生求職經驗的明唐學長、開車載我們繞環校完成實驗的 Judo 學 長、一起當助教帶實驗到半夜的耕維學長、在修 DSP 實驗時給予我許多幫助的昭男學 長。在同學中，感謝論文題目相近與坐在我後面的期元，讓我有個能隨時互相討論和分 享研究成果的對象。感謝曾經一起在地下室 B1 奮鬥一年的淵翰和孟瑋、先口試讓我們 見習的業文以及共同討論該如何規劃口試時程的冠宏和哲予。在學弟中，感謝工研院打 工時在相同計畫底下的凱傑、將延續這篇論文研究的凱祥、接下實驗助教的佑軒以及目 前擔任實驗室管理員的綜韓和瑋庭。感謝助理淑伶在採購實驗器材上的許多幫忙。 最後要感謝在求學過程中幫助過我的許多貴人們以及我的家人父親、母親、哥哥和姊 姊，如果沒有你們給我的支持與鼓勵，我是不可能走到今天這一步的，在此感謝你們給 予我的一切並獻上最誠摯的謝意。

iv

目錄

摘要 ... i Abstract ... ii 致謝 ... iii 目錄 ... iv 表目錄 ... vi 圖目錄 ... vii 第一章 緒論 ... 1 1.1 研究動機 ... 1 1.2 論文貢獻 ... 2 1.3 論文架構 ... 3 第二章 攝影機成像與幾何限制 ... 4 2.1 旋轉矩陣 ... 4 2.1.1 Euler angle ... 4 2.1.2 Quaternion ... 5 2.2 攝影機成像 ... 7 2.3 Epipolar geometry ... 10 2.4 Trifocal tensor ... 12 第三章 攝影機與慣性量測裝置的感測器融合 ... 18 3.1 慣性量測裝置的運動學方程式 ... 18

3.1.1 Nominal state kinematic ... 20

3.1.2 Error state kinematic ... 20

3.2 感測器校正方法 ... 23

3.2.1 攝影機校正 ... 23

3.2.2 攝影機與慣性量測裝置的空間關係校正 ... 26

3.2.3 整體校正流程 ... 29

3.3 感測器融合架構 ... 30

3.4 Visual odometry 與 Visual SLAM ... 31

3.5 Vision-aided inertial odometry ... 31

3.5.1 Multi-state constraint Kalman filter ... 33

v 3.5.3 整體測程器演算法 ... 40 第四章 軟硬體設計與實現 ... 41 4.1 硬體元件架構 ... 41 4.1.1 慣性量測裝置 ... 43 4.1.2 GPS ... 45 4.2 攝影機與慣性量測裝置的同步機制 ... 46 4.3 整體整合模組 ... 50 第五章 實驗結果與討論 ... 52 5.1 攝影機與慣性量測裝置校正結果 ... 52 5.2 視覺輔助慣性測程器 ... 56 5.2.1 KITTI dataset 的實驗結果 ... 56 5.2.2 xGIC 的實驗結果 ... 64 第六章 研究成果與未來展望 ... 68 6.1 研究成果 ... 68 6.2 未來展望 ... 69 Appendix ... 70 Reference ... 72

vi

表目錄

Table 4-1 MT9V034 規格表 ... 42 Table 4-2 ADIS 16480 規格表 ... 43 Table 4-3 GPS M2 規格表 ... 45 Table 5-1 攝影機與慣性量測裝置校正實驗的狀態估測結果表 ... 55

Table 5-2 KITTI dataset case1 的整體 RMSE 和終點誤差結果比較 ... 59

Table 5-3 KITTI dataset case2 的整體 RMSE 和終點誤差結果比較 ... 61

Table 5-4 KITTI dataset case3 的整體 RMSE 和終點誤差結果比較 ... 63

Table 5-5 xGIC 在室外實驗的整體 RMSE 和終點誤差結果比較 ... 66

vii

圖目錄

Fig. 2-1 針孔攝影機模型成像示意圖 ... 7

Fig. 2-2 影像平面座標系與攝影機成像座標系關係 ... 8

Fig. 2-3 世界座標系與攝影機座標系的空間轉換關係示意圖 ... 9

Fig. 2-4 epipolar geometry 示意圖 ... 10

Fig. 2-5 三張影像間 line-line-line 的對應關係[12] ... 12 Fig. 2-6 三張影像間 point-line-line 的對應關係[12] ... 16 Fig. 2-7 三張影像間 point-line-point 的對應關係[12] ... 17 Fig. 2-8 三張影像間 point-point-point 的對應關係[12] ... 17 Fig. 3-1 校正板與世界座標系定義 ... 23 Fig. 3-2 攝影機座標系

 

C 、慣性座標系

 

I 、世界座標系

 

G 與第i個特徵點之相對空間關係 ... 26 Fig. 3-3 校正流程方塊圖 ... 29 Fig. 3-4 感測器融合架構的分類[21] ... 30 Fig. 3-5 以 RANSAC 演算法決定的線段 ... 39 Fig. 4-1 cameleon 外觀 ... 41 Fig. 4-2 硬體平台架構圖 ... 42 Fig. 4-3 ADIS16480 外觀 ... 44 Fig. 4-4 ADIS16480 功能架構圖 ... 44 Fig. 4-5 ADIS16480 可程式化低通濾波器的頻率響應圖 ... 44 Fig. 4-6 GPS 模組外觀 ... 45 Fig. 4-7 Camera-IMU 同步機制系統架構圖 ... 46 Fig. 4-8 攝影機快門的曝光訊號區分為奇數與偶數 ... 47 Fig. 4-9 攝影機快門曝光時間與慣性量測裝置的取樣訊號同步結果 ... 47 Fig. 4-10 ADIS16480 的時序圖 ... 47

viii

Fig. 4-11 慣性量測裝置的取樣與完成取樣訊號 ... 48

Fig. 4-12 PicoBlaze SPI 傳輸測試... 49

Fig. 4-13 MicroBlaze 中斷訊號驗證 ... 49 Fig. 4-14 整體整合模組外觀 ... 50 Fig. 4-15 傳輸格式示意圖 ... 51 Fig. 5-1 校正實驗的實際影像... 52 Fig. 5-2 軌跡估測結果 ... 52 Fig. 5-3 攝影機與慣性量測裝置間的位移估測誤差 ... 53 Fig. 5-4 攝影機與慣性量測裝置間的旋轉角度估測誤差 ... 53 Fig. 5-5 加速規 bias 估測誤差 ... 54 Fig. 5-6 陀螺儀 bias 估測誤差 ... 54 Fig. 5-7 重力向量估測誤差 ... 55

Fig. 5-8 KITTI dataset 的實驗車 ... 56

Fig. 5-9 KITTI dataset 的影像範例以及挑選特徵點的方式 ... 57

Fig. 5-10 KITTI dataset case1 的移動軌跡估測結果 ... 58

Fig. 5-11 KITTI dataset case1 的 RMSE 結果比較 ... 59

Fig. 5-12 KITTI dataset case2 的移動軌跡估測結果 ... 60

Fig. 5-13 KITTI dataset case2 的 RMSE 結果比較 ... 61

Fig. 5-14 KITTI dataset case3 的移動軌跡估測結果 ... 62

Fig. 5-15 KITTI dataset case3 的 RMSE 結果比較 ... 63

Fig. 5-16 本論文實作的硬體 xGIC 裝設於汽車上的實際圖 ... 64

Fig. 5-17 xGIC 於戶外的拍攝圖 ... 64

Fig. 5-18 xGIC 在室外實驗的移動軌跡估測結果 ... 65

Fig. 5-19 xGIC 在室外實驗的 RMSE 結果比較 ... 66

1

第一章 緒論

1.1 研究動機

在近幾年中，隨著微機電製成技術的發展，使得慣性量測裝置的體積變小、價格降 低和能源效率提高等，而因為慣性量測裝置的改進，使得以慣性量測裝置為核心的慣性 導航系統(Inertial Navigation System, INS)被大量地用來估測載體(如智慧車、小型直升機 和各式機器人等)的移動軌跡，主要方式為利用對慣性量測裝置所測量到的加速度和角 速度積分來做航位推算(dead-reckoning)，也就得到載體的位置、速度和旋轉角度，但由 於慣性量測裝置的訊號會受到 bias 和雜訊的影響，造成在積分的過程中誤差不斷地累積， 最終導致所得的軌跡是相當不可靠的，為了解決這個問題，一些慣性導航系統會使用 GPS 的訊號來週期性地修正慣性量測裝置，也就是做慣性量測裝置與 GPS 的感測器融 合，但這個方式必須仰賴 GPS 訊號的存在，在室內、隧道、地底、外太空等沒有 GPS 訊號的環境下，這個方式是不可行的，而且 GPS 訊號的好壞會受到周圍環境的影響， 像是在建築物旁會有 multipath 的問題，另外高精準度的 GPS 價格是相當昂貴的。 而另一個用來修正慣性量測裝置的選擇是使用攝影機，攝影機的優點在於它的價格 較低、可提供豐富的二維量測資訊、體積小也易於安裝，攝影機利用追蹤在影像序列間 的特徵點可用來計算攝影機的移動軌跡[12]，而進一步的是，攝影機和慣性量測裝置是 互補的感測器[21]，攝影機在較慢的運動時，量測資訊的不確定性較低，慣性量測裝置 則在較快的運動時，量測資訊的不確定性較低，不僅如此，慣性量測裝置可以提供一般 單一攝影機會缺乏的地圖尺度，經過以上的考量，在本論文中考慮的是單一攝影機與慣 性量測裝置的感測器融合。 這兩種感測器的融合是相當熱門的研究，它可以提供許多的應用，像是：地圖尺度 估測[1][23]-[25]、影像解模糊[2]、輔助影像特徵點的追蹤[3][4]、地平面偵測[5]、移動 軌跡估測[19][31]-[34][36]-[38][40][41][45]等等，而在這一、兩年間，這個領域開始著手

2 解決初始條件的問題[6]-[10]。 而本論文的主要目的為估測移動軌跡，因此提出一套結合單一攝影機與慣性量測裝 置的測程器架構，由於是以三張影像中存在的攝影機幾何限制作為攝影機所提供的量測 資訊，使得本論文的方法可以不需要估測特徵點在空間中的位置，也就是不需要進行重 建環境的演算，同時我們將三張影像分別對應到的攝影機姿態在濾波器中修正，形成一 多個狀態限制的 Kalman filter，因此是一個在計算量與精確度間取得平衡 sliding-window 式測程法，最後為了有效地排除比對錯誤或是落於移動物體上的特徵點，我們以基於三 視角幾何的 RANSAC 演算法來挑選 inlier。

1.2 論文貢獻

本論文的貢獻如下: 1. 提出一套結合單一攝影機與慣性感測資訊的測程器架構 2. 以三視角幾何避免一般視覺式測程法或同時定位與地圖建立的方法中需要估測特 徵點在空間中的位置，也就是不需進行環境的重建，並且是以多個狀態限制卡爾曼 濾波器觀念設計的滑動視窗式測程法 3. 以基於三視角幾何的隨機抽樣一致演算法排除比對錯誤或是落於移動物體上的特 徵點，使得整體演算法在動態環境下仍有可靠的結果 4. 實現整合單一攝影機、慣性量測模組與 GPS 的硬體設備 5. 以真實環境下的資料實現提出之視覺輔助慣性測程演算法並探討其結果

3

1.3 論文架構

本論文主要涵蓋三個部份，分別為演算法介紹、軟硬體架構以及整體演算法的實驗 結果驗證，以下簡略描述其大致內容：  第一部份： 第二章：介紹常用的旋轉表示法，並敘述所使用的攝影機成像模型以及影像在不同視角 間存在的攝影機幾何限制(epipolar geometry 和 trifocal tensor)。

第三章：介紹慣性量測裝置的運動學方程式、攝影機的校正、攝影機與慣性量測裝置間 的校正、感測器融合的架構以及 visual odometry 與 visual SLAM，接著利用攝影機幾何 限制，本論文提出一個不需重建環境的視覺輔助慣性測程器，並以基於三視角幾何的 RANSAC 排除比對錯誤或是落於移動物體上的特徵點。  第二部份： 第四章：介紹實驗用的攝影機、慣性量測裝置和 GPS 硬體設備，以及本論文所實作的 整合模組。  第三部份： 第五章：提出校正與視覺輔助慣性測程器於真實環境下的實驗結果，並討論其結果。 第六章：總結本論文的研究成果以及未來展望。

4

第二章 攝影機成像與幾何限制

2.1 旋轉矩陣

由於本論文主要在探討剛體(rigid body)在空間中的運動軌跡，所謂的軌跡包含剛體 的旋轉與位移，而旋轉牽涉到不同的表示方法，因此以下將簡介常用來表示旋轉的 euler angle 和 quaternion 方法。

2.1.1 Euler angle

在表示旋轉的方式中，euler angle 是最簡單同時也最直觀的方法，它將在三維空間 上的旋轉以分別對三個軸的旋轉角度來表示，像是沿著x軸旋轉 角度的旋轉矩陣為：

 

1 0 0 0 cos sin 0 sin cos x          _  _     R (2.1) 沿著y軸旋轉



角度的旋轉矩陣為：

 

cos 0 sin 0 1 0 sin 0 cos y                 R (2.2) 沿著z軸旋轉角度的旋轉矩陣為：

 

cos sin 0 sin cos 0 0 0 1 z                 R (2.3) 藉由組合Rx

 



、Ry

 



和Rz

 



可以旋轉到空間中的任意角度，但因為矩陣的相乘並 不具有交換性，因此先旋轉哪一軸對於最後的結果會有所差異，常見的是先旋轉x軸， 再旋轉y軸，最後旋轉z軸，因此所得的旋轉矩陣為：

5

     

cos cos sin sin cos cos sin cos sin cos sin sin cos sin sin sin sin cos cos cos sin sin sin cos

sin sin cos cos cos

z  y  x                                      _   _      R R R R (2.4)

而 euler angle 的最大問題是在於它會有 singular 的情形，導致旋轉角度無法從旋轉矩陣 中得到唯一解。

2.1.2 Quaternion

Quaternion 是另一種常用來表示旋轉的方式，相較於 euler angle 在計算上會面臨到

cos和sin函數的計算，quaternion 只牽涉到代數的相乘與相加，而 quaternion 並不會有

singular 的情形，它的概念是將旋轉以一個旋轉軸和旋轉角度表示，其基本定義如下： 2 2 2 1 1 1 Q a bi cj dk i j k           (2.5) 其中



a b c d, , ,



為實數，若是 unit quaternion 的話，必須滿足 2 2 2 2 1 a b  c d  ，接著利 用右手法則定義



i j k, ,



的關係如下： ij ji k jk kj i ki ik j          (2.6) 為了表示上的方便，將Q的虛部 (bi cj dk )以向量的形式描述： a a b c d         _{   }       q q (2.7) 若是給定一單位長度的旋轉軸u與對該軸的旋轉角度



，利用 quaternion 可表示為[11]： cos 2 sin 2                q u (2.8)

6

而在 unit quaternion 的假設下，將向量x旋轉到向量x'可用下列的 quaternion rotation

operator 表示：

*

'   'R( )

x q x q x q x (2.9)

其中x



0 x



T； *

q 為 conjugated quaternion；為 quaternion product， *

q 和的定義 如下： *  a   _   q q (2.10)







1 1 1 1 2 2 2 2 1 1 1 1 2 2 2 2 1 2 1 2 1 2 1 2 1 2 1 2 1 2 1 2 1 2 1 2 1 2 1 2 1 2 1 2 1 2 1 2 a b i c j d k a b i c j d k a b i c j d k a b i c j d k a a b b c c d d a b b a c d d c a c b d c a d b a d b c c b d a                       _{  }           _{  }    1 2 1 2 q q q q (2.11) 利用(2.10)式和(2.11)式重新整理(2.9)式，可得R q( )為：

































































2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 3 2 2 ( ) 2 2 2 2 2 1 2 2 2 2 1 2 2 2 2 1 2 1 2 T 2 a b c d bc ad bd ac R bc ad a b c d cb ab bd ac cd ab a b c d a b bc ad bd ac bc ad a c cb ab bd ac cd ab a d a a          _      _  _{    }     _{   }              _{   }        _ _ q I qq q (2.12) 其中， 2 2 3 2 1 0 0 0 0 1 0 , , 0 0 0 1 0 T b bc bd d c bc c cd d b bd cd d c b              _{ } _{ } _  _{ }  _             I qq q (2.13)     q 為 skew-symmetric matrix，可代表矩陣的外積。

7

2.2 攝影機成像

本論文採用在電腦視覺領域中廣泛被使用的針孔攝影機模型(pinhole camera model) [12]，其成像原理是將三維空間座標依照透視投影法(perspective projective)投影至二維平

面上，如 Fig. 2-1 所示。在二維平面的座標定義中是以焦距面Z  f 作為其平面，並且

以攝影機的投影中心作為其原點。

Fig. 2-1 針孔攝影機模型成像示意圖

圖中C為攝影機的焦點(focal point)、P為主點(principal point)、I 為影像平面、f 為焦距

(focal length)，在透視投影法下，三維空間座標M 



X Y Z



T投影至影像平面 Z  f 上， 依照相似三角形關係可得：





T X Y T u v f f Z Z    _{  }   m (2.14) 依照 homogeneous coordinate 定義，可將(2.14)式表示為以下關係： 0 0 0 0 0 0 1 1 0 0 1 0 1 X u fX f Y s s v fY f Z Z        _{  }   _{       }  _{ } _{  } _{ } _{  }   _{       }         m m PM (2.15)



X Y Z 1



T  T 1T  _{  } M M (2.16)

C

Z

Y

X

f

M

m

P

I

u

v

8 考慮實際影像平面的主點與攝影機的焦點並不重合，如 Fig. 2-2，因此：





0 0 T T X Y u v f u f v Z Z    _   _   m (2.17) Fig. 2-2 影像平面座標系與攝影機成像座標系關係 利用(2.17)式，可將(2.15)式改寫為： 0 0 0 0 0 0 0 0 1 0 0 1 0 1 X fX Zu f u Y s fY Zv f v Z Z       _{  }      _{  }  _{  }  _{ } _{  }   _  _{    }   m m PM (2.18)

定義攝影機內部參數矩陣(Intrinsic Parameter Matrix)K並以K 表示投影矩陣P如下：

0 0 0 0 0 0 1 f u f v             _ _ K P K I 0 (2.19) 若考慮到成像像素的歪斜參數 與焦距在X 和Y軸有不同長度 f 和_x f_y的影響，可將內 部參數矩陣以更廣義的方式表示： 0 0 0 0 0 1 x y f u f v             K (2.20)

u

v

X

Y



u v

0

,

0



9 Fig. 2-3 世界座標系與攝影機座標系的空間轉換關係示意圖 然而在實際狀況下，世界座標系並不一定會定義在攝影機座標系上，使得兩座標系的參 考原點並不相同，如 Fig. 2-3，因此必須考慮兩者間的空間轉換關係： C   _ _ m PM K I 0 M (2.21) 0 1 0 1 C C G T G T G C  G GG  C  C CG         R p R R p M M M (2.22) 其中C G R 為3 3 旋轉矩陣，表示將世界座標系旋轉至攝影機座標系；C G p 表示世界座標 系的參考原點在攝影機座標系上的位置。C G R 與Cp_G定義了攝影機與世界座標的空間關 係，一般而言，此兩參數稱為攝影機的外部參數(extrinsic parameters)，以外部參數可改 寫(2.19)式的投影矩陣P為： 0 1 C C C G G G C C G G G     _ _  _{   }        R p m PM K I 0 M K I 0 M K R p M (2.23) 經過以上討論，(2.23)式代表攝影機在針孔成像模型假設下的一般投影關係式。 C f I u v G_X G_Y G Z C X C Y

,

G G C C

R

p

M m C_Z P

10

2.3 Epipolar geometry

在由兩個不同位置的攝影機(stereo camera)或是同一個攝影機在兩個不同位置所拍 攝的兩張影像間存在著一種 epipolar geometry 的幾何限制關係，如 Fig. 2-4，在電腦視覺 領域中，經常利用這種限制來加快特徵點的搜尋時間[12]。

Fig. 2-4 epipolar geometry 示意圖

圖中I 和₁ I 分別為攝影機在₂ C 和₁ C 兩個不同位置下的影像平面，₂ m 和₁ m 則分別為三₂ 維空間座標M投影到I 和₁ I 影像平面的像素位置，接著假設線段₂ C M₁ 上有任意一點 ' M ，M 投影到' I 影像平面的像素位置為₂ m ，由於₂' l_{m 2}是線段C M₁ 的投影結果，使 得m 必定會落在₂' l_{m 2}上，而這種必定的限制關係就稱為 epipolar geometry，反之，l_{m 1}也 是同樣的道理，因此l_{m 1}和l_{m 2}被稱為 epipolar line，而線段C C_{1 2}與I 和₁ I 影像平面分別₂

相交於e 和₁ e ，₂ e1和e2稱為 epipole，其代表的意思是I1和I2影像平面上的 epipolar line

必定通過它。Epipolar geometry 的數學關係式可以由向量C C_{1 2}、C M₁ 和C M₂ 所形成的 epipolar plane 開始推導：





0   2 1 2 1 C M C C C M = (2.24) 1 C e1 e2 C2 1 m m2 M m1 l lm2 1 I I2 ' 2 m ' M Epipolar plane

11 假設世界座標系定義在攝影機C₁上，而攝影機C₁和C₂間存在著一個旋轉矩陣 R 與位移 向量 t 的關係， R 與 t 是攝影機C2相對於攝影機C1的座標轉換，同時假設攝影機經過校 正，並且以攝影機內部參數矩陣將像素位置做 normalized，令m₁ 



u₁ v₁ 1



T，





2 2 2 1 T u v  m ，則： 2 2         1 2 1 1 2 2 C C t C M m C M M C Rm t t Rm (2.25) 利用上述關係式，改寫(2.24)式如下：







2

 

1



2



1



0 T _{T T}       2 1 2 1 C M C C C M = Rm t m m R t m (2.26) 矩陣的外積可以表示成 skew-symmetric matrix 的形式: 0 0 0 z y z x y x t t t t t t               _    t (2.27) 利用(2.27)式可改寫(2.26)式為： 2 1 2 1 0 T T   T      m R t m m Em (2.28) 其中E RT _{ }t 稱為必要矩陣(essential matrix)。由於(2.28)式是在假設像素位置已做 normalized 的前提下所推導的結果，若是假設像素位置未做 normalized，也就是必須考 慮攝影機的內部參數矩陣 K ，則： 1 1 1 2 2     1 m K m m K m (2.29) 將(2.29)式代入(2.28)式： 1 2 1 2 1 2 1 0 T  T T   T  m Em m K EK m m Fm (2.30) 其中FK EKT 1K RT T  _{ }t K1稱為基礎矩陣(Fundamental matrix)，而Fm₁為

12

2.4 Trifocal tensor

Trifocal tensor 是一種在三張影像間存在的幾何限制關係，它所代表的意義類似於在

兩張影像間存在的 epipolar geometry，但 trifocal tensor 與 epipolar geometry 不同的地方 在於，兩張影像的 epipolar geometry 只能限制第一張影像的特徵點會落在第二張影像的 某一條線上，而三張影像的 trifocal tensor 則可以在不知特徵點三維空間座標的情形下限 制特徵點在第三張影像上的像素位置，因此本論文利用此特性發展一套不需重建環境的 視覺測程器。Trifocal tensor 的數學式可由三維空間上的線段在三個不同攝影機位置上的 投影開始推導[12]，如 Fig. 2-5： Fig. 2-5 三張影像間 line-line-line 的對應關係[12] 圖中L為三維空間上的線段；l₁、l₂和l₃分別為線段L在攝影機位置C₁、C₂和C₃上 的投影。假設攝影機經過校正，則攝影機在C₁、C₂和C₃位置的投影矩陣各別為 1    P I 0 、P₂  _A a₄_和P₃  _B b_4，其中A和B為3 3 大小的矩陣，而a_i和b_i分別 表示為P₂和P3矩陣的第 i 個 column vector。

L

1

C

2

C

3

C

1

l

2

l

3

l

13 令X為線段L上的任意點，將X投影在攝影機畫面上，則l₁、l₂和l₃與投影點的內積 為零，如下： 1 1 2 2 3 3 0 0 0 T T T    l P X l P X l P X (2.31) 定義M為：





1 1 2 2 3 3 1 2 3 4 3 4 2 1 2 3 T T T T T T T              M P l P l P l l A l B l 0 a l b l m m m (2.32) 其中M為4 3 大小的矩陣。利用(2.32)式改寫(2.31)式可得： 0 T  M X (2.33) 線段L上的任意點X可以用兩個線性獨立的X₁、X₂向量來線性組合： 1 2     X X X (2.34) 將(2.34)式代入(2.33)式可得： 1 0 2 0 T  T  M X M X (2.35) 因此 T

M 的 null space 大小為 2，而根據 rank–nullity theorem，M的 rank 大小為 2，所以

M的某一個 column vector 必定可以由另外兩個 column vector 線性組合來得到，利用這

個關係假設m₁m₂m₃，觀察M的第 4 個 row vector 可得： 4 3 4 2 T T  b l   a l (2.36) 將(2.36)式和m₁m₂m₃的關係代入M的前 3 個 row vector 可得：

















1 4 3 2 4 2 3 3 4 2 2 4 3 T T T T T T T T     l b l A l a l B l l b A l l a B l (2.37)

14 則l₁的第i個分量可以表示為：





















1 3 4 2 2 4 3 2 4 3 2 4 3 2 4 4 3 2 3 T T T T i i i T T T T i i T T T i i T i l        l b a l l a b l l b a l l a b l l b a a b l l T l (2.38) 其中 ₄ T ₄ T i  i  i T b a a b ，因此l₁為：





1 2 1 2 2 2 3 3 2 1 2 3 3 T T T T T       l l T l T l T l l T T T l (2.39) 其中



T₁ T₂ T₃



即是 trifocal tensor 的表示法。上述(2.39)式所考慮的是三張影像間

line-line-line 的對應關係，而仍有 point-line-line、point-line-point 和 point-point-point 的關

係，分別如 Fig. 2-6、Fig. 2-7 和 Fig. 2-8 所示。以下將先考慮 point-line-line 的對應關係，

如 Fig. 2-6 所示，假設有一點x₁落在先前 Fig. 2-5 的l1上，則： 1 1 1 1 0 T i i i x l 



 x l (2.40) 將(2.38)式代入(2.40)式可得： 1 1 1 2 3 2 1 3 0 T T i i i i i i i i i x l  x  _ x _   





l T l l



T l (2.41) (2.41)式即為三張影像間 point-line-line 的對應關係。接著考慮 point-line-point 的關係， 如 Fig. 2-7 所示，假設x₁和x₃分別落在先前 Fig. 2-5 的l₁和l₃上，則： 1 1 3 3 0 T  T  x l x l (2.42) 由於x₁和x3存在著一個 homography matrix H 的關係： 3  1 x Hx (2.43) 將(2.43)式代入(2.42)式可得：





1 1 1 3 1 3 0 T T   T T  x l Hx l x H l (2.44)

15 因此可得 ₁ T ₃ l H l 的關係，而HT可由(2.38)式來得到： 2 1 2 2 2 3 T T T T            l T H l T l T (2.45) 則H為： 1 2 2 2 3 2 T T T      H T l T l T l (2.46) 再將(2.46)式代入(2.43)式可得三張影像間 point-line-point 的對應關係，如下： 3 1 1 2 2 2 3 2 1 1 2 T T T T i i i      _{    } 



 x Hx T l T l T l x x T l (2.47) 最後考慮 point-point-point 的關係，如 Fig. 2-8 所示，假設有x₂和任意一點y₂落在(2.47) 式中的l₂上，則l2可表示為： 2   2 2  2 2 l x y x y (2.48) 而利用向量本身的外積為零向量的特性可得：



3 3





3 3



3 3 1 3 T T T    _ _  _  _ x x x x x x 0 (2.49) 將(2.47)式代入(2.49)式： 3 3 1 2 3 2 1 3 1 3 T T T T i i i i i i                                 







x x x T l x l x T x 0 (2.50) 將(2.48)式代入(2.50)式：



2 2



1 3 2 2 1 3 1 3 T _T i i i i i i                    x y _



x T _x  y x _



x T _x  0 (2.51) 由於y₂是l2上的任意一點，而l2是任意一條通過x2的線，因此(2.51)式和 y2無關，則： 2 1i i 3 3 3 i               



 x x T x 0 (2.52)

16 以下總結 trifocal tensor 在三張影像間不同的對應關係：  Line-line-line：





1 2 1 2 3 3 T  T l l T T T l (2.53)  Point-line-line： 2 1 3 0 T i i i x   _   



 l T l (2.54)  Point-line-point： 3 1 2 T i i i      



 x x T l (2.55)  Point-point-point： 2 1i i 3 3 3 i           x _



x T _x  0 (2.56) Fig. 2-6 三張影像間 point-line-line 的對應關係[12]

L

1

C

2

C

3

C

3

l

X

1

x

2

l

2

x

3

x

17 Fig. 2-7 三張影像間 point-line-point 的對應關係[12] Fig. 2-8 三張影像間 point-point-point 的對應關係[12]

L

1

C

2

C

3

C

X

1

x

2

l

2

x

3

x

1

C

2

C

3

C

X

1

x

2

x

3

x

18

第三章 攝影機與慣性量測裝置的感測器融合

3.1 慣性量測裝置的運動學方程式

由於本論文是利用慣性量測裝置中的加速規與陀螺儀的量測資訊來做航位推算 (dead-reckoning)，因此必須對加速規與陀螺儀的量測資訊建立模型，並描述它的運動學 方程式(kinematic)，首先令加速規與陀螺儀的量測資訊分別為a_m和ω_m，如下：

 



T G G G m R I   a a a q a g b n (3.1) I m  g g ω ω b n (3.2) 其中G I q 為慣性量測裝置在世界座標系下的旋轉角度的 quaternion 表示法，R

 

Gq_I 為其 旋轉矩陣；G a為線性加速度；G g為重力方向；ba和b 分別為加速規和陀螺儀的 bias；g I_ω 為角速度；n_a和n 分別為加速規和陀螺儀的量測雜訊，並假設雜訊的分布為平均值_g 為零的 Gaussian distribution。而用來描述慣性量測裝置的狀態，一般會包含它在世界座 標系上的位置、旋轉角度、速度以及本身的 bias，令x為其狀態，如下： T G T G T G T T T I I I a g      x p q v b b (3.3)

假設慣性量測裝置的 bias 是 random walk process，並由平均值為零 Gaussian distribution

的雜訊n_ba和n 所驅使，則根據力學方程式可得其動態模型如下： _bg G G I  I p v (3.4)

 

1 1 2 2 G I G G I I   I  I  q ω q q ω (3.5) G G I  v a (3.6) a  ba b n (3.7)

19 g  bg b n (3.8) 其中，

 

0 I T I I I             _{ }   ω ω ω ω (3.9) 0 I I        ω ω (3.10)

(3.4)式至(3.8)式可稱為慣性量測裝置的 true state kinematic，由於在本論文中，慣性量測

裝置的狀態會作為濾波器的狀態，為了最小化濾波器的狀態維度以及線性化的需求[13] [14]，將 true state 以大訊號的 nominal state 和小訊號的 error state 表示，如下：

ˆ G G G I  I  I p p p (3.11) ˆ G G I  I



q q q (3.12) ˆ G G G I  I  I v v v (3.13) ˆ a  a a b b b (3.14) ˆ g  g  g b b b (3.15) 其中， 1 1 2 G I            q θ (3.16) 而 ˆG I p 、Gqˆ_I、 ˆG I

20

3.1.1 Nominal state kinematic

由於雜訊n_a、n 、_g n_ba和n 的平均值皆假設為零，因此將 true state kinematic 取期_bg

望值即可移除雜訊的影響，並得到 nominal state kinematic，如下：

ˆ ˆ G G I  I p v (3.17)

 

1 1 ˆ _ˆ ˆ ˆ ˆ 2 2 G G G I   I  I  q ω q q ω (3.18)

 

ˆ ˆ ˆ G G G I R I  v q a g (3.19) 3 1 ˆ a   b 0 (3.20) 3 1 ˆ g   b 0 (3.21) 其中， ˆ ˆ _m _a a a b (3.22) ˆ ˆ  _m _g ω ω b (3.23)

3.1.2 Error state kinematic

完整的 error state kinematic 如下：

G G I  I p v (3.24) ˆ G G I I g g θ  _ω_ θ  b n (3.25)

 

ˆ _ˆ

 

ˆ

 

ˆ G G G G G I  R I    I R I a R I a v q a θ q b q n (3.26) a  ba b n (3.27) g  bg b n (3.28)

21

(3.24)式、(3.27)式和(3.28)式可由 true state kinematic 減去 nominal state kinematic 得到，

而(3.25)式和(3.26)式需要額外的推導，以下將先推導(3.25)式，也就是角度的 error state kinematic，首先令角速度的 error



ω為： g g ω  b n (3.29) 則角速度的 true stateI_{ω可表示為：} ˆ I_{ω ω} _ω (3.30) 將(3.12)式代入(3.5)式可得：



_ˆ



_{ˆ ˆ} 1 2 G G G G G I I  I  I  I  I q q q q q q q q ω (3.31)





1 1 ˆ ˆ ˆ ˆ 2 2 G G I G G I G I  I  I  I  I  q q q ω q q q ω q ω q (3.32) (3.32)式左右同乘GˆI 1  _ q 可得：





















1 1 _{ˆ ˆ} 2 1 _ˆ 2 0 _{0 ˆ} 1 ˆ ˆ 2 ˆ 0 1 2 _{ˆ ˆ} G G I I I I I T _T I I T I I I                              _{ }           _{ }      _{   }      q q q ω ω q q ω ω q ω _ω q q ω ω ω ω ω ω q ω ω ω ω (3.33) 將(3.30)式代入(3.33)式可得：









0 1 ˆ 2 2 1 0 1 1 ˆ 2 2 2 1 1 ₂ 1 2 ˆ 2 T T G I T G I G G I I                            _{ }       _{ }   _{ }       _{ }  _{  }  _        _{    }          ω q q ω ω ω ω θ ω ω ω ω θ ω ω θ ω θ (3.34)

22 假設 T G I ω θ 和 G I







   ω  θ 可以近似為零，而q為： 0 1 ₀ 1 1 1 _ˆ 2 2 2 G G G I I I          _{ }     _{  } _{  }  _ _ _{ }       q ω ω θ θ θ (3.35) 則G I θ 為： ˆ G G I I



θ  _ω_



θ 



ω (3.36)

將(3.29)式代入(3.36)式可得角度的 error state kinematic，如(3.25)式。接著推導(3.26)式，

也就是速度的 error state kinematic，首先令加速度的 error



a為：

a a



a  b n (3.37) 則加速度的 true state a_t可表示為：

 

G



ˆ



t R I  a q a a (3.38) 將(3.13)式和(3.38)式代入(3.6)式可得：

 





ˆ ˆ G G G G I I R I   v v q a a g (3.39) 將(3.19)式代入(3.39)式可得：

 

Gˆ ˆ G

 

G



ˆ



I I I R q a v R q aa (3.40) 由於Gq_I  Gqˆ_I



q，假設G I θ 的值很小，則

 

G I R q 可以近似為：

 

G

 

Gˆ



G



I I I R q R q I_ θ _ (3.41) 將(3.41)式代入(3.40)式可得：

 

Gˆ _ˆ G

 

Gˆ



G





_ˆ



I I I I R q a v R q I_ θ _ aa (3.42) 將左右兩邊的

 

Gˆ ˆ I R q a消掉，並假設GθIa可以近似為零，則：

 

ˆ _ˆ

 

ˆ G G G G I  R I    I R I  v q a θ q a (3.43)

23

3.2 感測器校正方法

3.2.1 攝影機校正

攝影機的校正採用[15]的方法，其架構主要源自[16]，[15][16]的方法主要概念皆為 利用校正板作為空間中已知的座標資訊來達到校正攝影機內部參數的目的，在有校正板 的情形下，將世界坐標系的 XY 平面定義在校正板上(如 Fig. 3-1)，因此校正板上的角點 投影在攝影機畫面中的位置可用以下關係式描述：













0 0 1 1 1 1 1 X X u X X Y Y v    Y Y       _{   }      _{    }  _     _{   }       _{   }               1 2 3 1 2 K R t K r r r t K r r t H (3.44)



 



1121 1222 1323 31 32 33 h h h h h h h h h        _{  }     1 2 1 2 3 H K r r t h h h (3.45)

H為根據一個 scale factor



所定義的3 3 homography matrix，文獻上有許多的方法可

以用來估測H[16]。 X Y Z O Fig. 3-1 校正板與世界座標系定義 利用任何旋轉矩陣R必定是 orthonormal matrix 的特性，可以得到以下兩條關係式： 1 1 2 0 T T  _ h K K h (3.46)

24 1 1 1 1 2 2 T T T   _    h K K h h K K h (3.47)

(3.46)式為旋轉矩陣的 column vector 彼此互相垂直的性質，(3.47)式為 column vector 長

度為 1 的性質。由於(3.46)式和(3.47)式皆存在 T 1 K K ，因此令A如下： 11 12 13 1 12 22 23 13 23 33 T A A A A A A A A A         _{  }     A K K (3.48)

利用A是 symmetric matrix 的性質，將A以一個6 1 的 column vector 描述：



11 12 22 13 23 33



T A A A A A A  a (3.49) 因此在給定 homography matrix H以及(3.46)式和(3.47)式兩個條件下，可以得到以下關 係式：





12 11 12 T T           v a 0 v v (3.50) 1 1 1 2 2 1 2 2 3 1 1 3 3 2 2 3 3 3 T ij h hi j h hi jh hi j h hi j h hi j h hi j h hi jh hi j h hi j v (3.51)



1 2 3



T i i i i

h  h h h 為 homography matrix H的第i個 column vector。假設有 n 張影像，

將(3.50)式依序堆疊可得：  Va 0 (3.52) 其中，矩陣

V

維度為

2

n



6

，在

n



3

的條件下，可以求得未知向量a的唯一線性解，而 得到向量a後，透過(3.48)式得到以下關係式計算攝影機內部參數K ： 0 0 0 0 0 1 c u v              K (3.53)







2



0 12 13 11 23 11 22 12 v  A A A A A A A (3.54)





2 33 13 0 12 13 11 23 11 A A v A A A A A   _   _ (3.55)

25 11 A    (3.56)



2



11 11 22 12 A A A A     (3.57) 2 12 c A    (3.58) 2 0 0 13 u cv A   (3.59) 一旦求得攝影機內部參數後，則攝影機外部參數的旋轉矩陣R與位移向量t可由以下關 係式求得：



 



-1 -1 -1 -1 1 2 1 2          R = K h K h K h K h (3.60) -1 3   t K h (3.61) -1 -1 1 2 =1 =1  K h K h (3.62) 截至目前為止，對於攝影機內部與外部參數的解是在線性模型下所得到的，雖然線 性解有計算速度快的優點，但在真實環境下，線性解會受到鏡頭失真(主要為 radial distortion)與雜訊干擾造成所得到的線性解是不可靠的[17]，因此為了考慮鏡頭失真與雜 訊干擾，必須以非線性最佳化的方式求解。假設鏡頭失真的參數為：



1 2 3 4 5



c kc kc kc kc kc k (3.63) 而原始像素位置x、考慮鏡頭失真的像素位置x_d與鏡頭失真參數的關係[15]為： u v        x (3.64)



2 4 6



1 2 5 1 d d c c c d u k r k r k r d v   _{ }       x x x (3.65) 2 2 2 r u v (3.66)









2 2 3 4 2 2 3 4 2 2 2 2 c c c c k uv k r u d k r v k uv  _{ }      _{ }    x (3.67) c k 可以由(3.65)式推導得到一個最小平方法的線性解。將所有需要估測的參數以非線性

26 目標函數描述，接著以基於投影誤差最小化的方式來達到參數最佳化的目的：





2 1 1 ˆ , , , , n m ij c i i j i j 



m m K k R t M (3.68) 其中m_ij為在第i個攝影機姿態下，第 j個角點M_j的實際像素位置， ˆm 則為在考慮鏡頭 失真後的投影像素位置。(3.68)式為一非線性最佳化的問題，可以由 Gauss-Newton 或 Levenberg-Marquardt 演算法求解，而最佳化所需的K k, _c,



R t_i, _i i1 n



初始值則以先前 推導的線性解來得到，經迭代收斂後，所得的值即為此演算法對於攝影機參數校正的結 果[16]。

3.2.2 攝影機與慣性量測裝置的空間關係校正

一般來說，攝影機和慣性量測裝置的空間關係(如 Fig. 3-2)可以從實際硬體的設計圖 來得知，但由於硬體製造上的諸多不確定性，導致兩者之間的空間關係不夠精確，而空 間關係的誤差，會造成後續感測器融合時兩者的資訊不一致以及不準度，因此要能使攝 影機與慣性量測裝置平台能夠確實地發揮作用，必須對兩者間的空間關係進行校正。在 此階段的校正，是假設攝影機內部參數與慣性量測裝置的偏軸、偏心參數已校正過，因 此需要校正的參數為攝影機與慣性量測裝置間的旋轉矩陣和位移向量，以及慣性量測裝 置的加速規和陀螺儀 bias。 Fig. 3-2 攝影機座標系

 

C 、慣性座標系

 

I 、世界座標系

 

G 與第i個特徵點之相對空間關係  I  C C x C y C z I x I z I y  G G z G x G y , I I C C R p , G G I I R p G Li p

27

在攝影機與慣性量測裝置現存的校正方法中，Mirzaei 等人於 2008 年提出的方法[18] 是截至目前為止較方便且較精準的方法，他們的方法不需要額外昂貴的特殊儀器來輔助 校正的過程，其方法使用 extended Kalman filter 作為演算法的主要核心，將需要校正的 bias、旋轉與位置關係作為濾波器的狀態。其後 Kellyt 等人提出改以 unscented Kalman

filter 作為核心，並將重力方向作為估測狀態的校正方法[19]，unscented Kalman filter 除

了可以避免計算 Jacobian matrix，在面對此高度非線性的系統上，相較 extended Kalman filter 有較穩定的收斂性和準確性，因此本論文主要採用 Kellyt 等人的方法[19]來校正攝

影機與慣性量測裝置的空間關係。

此部分的校正方法為將一已知維度的校正板作為世界坐標系的參考點，在攝影機可 以觀測到校正板的範圍內，任意移動攝影機與慣性量測裝置平台，接著以移動過程中所 紀錄的校正板角點的像素位置，與同步量測的慣性裝置資訊，將需要校正的空間關係與 bias 作為估測的狀態，使用 unscented Kalman filter 進行估測。

此系統待估測狀態向量X為： T G T G T G T T T I T I T G T I I I a g C C      X p θ v b b p θ g (3.69) 其中G I p、Gθ_I和Gv_I各別代表慣性量測裝置在世界坐標系的位置、旋轉角度和線性速度； a b 和b_g分別為加速規和陀螺儀的 bias；IpC和 I C θ 分別為攝影機相對於慣性量測裝置的 位置和旋轉角度；G g表示在世界坐標系下的重力向量。 假設雜訊為 Gaussian distribution，表示為下：





















2 2 2 2 2 ~ 0, ~ 0, ~ 0, ~ 0, ~ 0, a a g g ba ba bg bg i i N N N N N      n n n n η (3.70) 此系統於連續時間下的動態模型為：

 

1

 

G G G G I G G I G I I I I I I a ba g bg I I G C C S  R          p v θ θ ω v θ a g b n b n p 0 θ 0 g 0 (3.71)