視覺慣性輪速里程計

摘要：結合各種感測器的里程計算器，是自駕車必備的基礎元件。在各類里程計中，以相機與慣性測量單元 (Inertial Measurement Unit, IMU) 組成的視覺慣性里程計最常被使用，因其在感測器的尺寸與價格上具有優勢。然而，視覺慣性里程計仍有所限制，例如在車輛等速行駛下，尺度將變得不可觀測。因此，有些方法將輪速編碼器和視覺慣性里程計做結合來達到更準確和強健的表現。本文將介紹我們基於開源視覺慣性里程計架構所開發的視覺慣性輪速里程計。
Abstract：Combining different sensors to compute odometry is a fundamental component of self-driving cars. Among different types of odometry, visual-inertial odometry (VIO) that utilizes a camera and an inertial measurement unit is popularly used due to its advantages of sensor size and cost. However, VIO still has some limitations, e.g., scale is unobservable under constant velocity motion. Therefore, several methods have been developed to fuse wheel speed sensors and visual-inertial odometry to achieve more accurate and robust performance. In this article, we will present a visual-inertia-wheel odometry based on an open-source VIO architecture.

關鍵詞：視覺慣性輪速里程計、慣性測量單元、輪速編碼器
Keywords：Visual-inertial-wheel odometry (VIWO), Inertial measurement unit (IMU), Wheel encoder

前言
視覺慣性里程計，雖然解決了單目相機尺度(scale)與慣性感測器偏差值的估計問題，但由於慣性感測器測量的是加速度，在等速運動時，系統尺度變得不可觀測(unobservable) [1]。在車輛定位的應用中，這樣的情形時常發生。因此，需要額外的速度資訊，使系統尺度總是可觀測。另外，我們在實測中也發現視覺慣性里程計在車輛停止時會有估測位置漂移，以及在開闊場域時，遠處影像特徵點不確定性較大的問題。為了解決上述問題，我們將輪速計加入系統，並修改慣性感測器處理與影像特徵點處理應對這些場景。
概念
在先前的視覺慣性里程計(Visual-inertial odometry, VIO)[2]中，我們已經對各大開源的VIO算法做了基本介紹。在本文中我們將詳細介紹如何使用最佳化方法將不同感測器融合，並且修改VINS-Mono[3]的架構，使其能夠融合輪速編碼器的資訊。在介紹之前，我們先定義一些符號與座標系統。我們使用tk代表相機拍攝到第ｋ個關鍵幀(keyframe)的時間點，(·)代表我們要描述的變數。我們常將其它感測器的座標系轉換到IMU座標系，因此車體座標系與IMU座標系是相同的。我們使用上標表示變數所在的座標系，例如在tk時的IMU/車體座標系為(·)bk、相機座標系為(·)ck 和輪速編碼器座標系為(·)ek，以上三個座標系隨車輛移動。(·)w為世界座標系，世界座標系是由初始車體座標系(·)b0過旋轉將其z軸對齊重力方向而得，即gw=[0  0 9.8]T。我們使用下標表示變數來自於哪個感測器量測所得，例如(·)bk示在tk時由IMU所量到或計算得出的變數。同理，(·)ck和(·)ek分別表示相機及輪速編碼器在tk時所量到或計算得出的變數，Pcb,Rcb分別為相機-IMU間的外部參數中的位移量和旋轉量，它們可用齊次矩陣來表示。除了使用旋轉矩陣R之外，我們也使用四元數(quaternion) q來表示旋轉。代表含有雜訊的感測數據或估計值。我們接下來會介紹IMU、輪速編碼器及相機的測量模型。
我們使用代表在t時刻下世界座標系到IMU坐標系的旋轉矩陣，IMU測量結果則分別為加速度與角速度，其測量值包含了加速度偏差值bat加速度雜訊nat、角速度偏差值bωt及角速度雜訊nωt，可表示如下：

(1)(2)
將一段時間的測量結果積分，即可求得車輛的位姿 (pose)。但此種方法依賴系統的上一個時刻的位置與速度，且當上一時刻的位姿因最佳化改變時，需要花費時間重新積分來計算當前時刻的位置與速度。因此使用預積分[4]技巧，用一段時間內的IMU數據計算出相對於上一時刻位置、速度和朝向的相對變化量，而此變化量與上一時刻的位姿無關，所以不須重新做積分運算。我們使用α, β及γ代表IMU預積分之結果，其意義為位置、速度和朝向的變化量。
一般我們使用兩個輪速編碼器分別測量兩個輪胎的旋轉速度(ωr ,ωl)，可以利用輪胎半徑(rl ,rr  )和車輛基準長度(b, 左右兩輪間中心的直線長度)計算出車輛朝向方向(x軸)的速度與Yaw的角度變化量，如下式：
 

          (3)
相機感測的模型的部份，我們使用可利用運動推斷結構(Structure from Motion, SfM)估測相機姿態與特徵點的3D座標。我們使用角點為特徵點，並且使用光流法進行追蹤。與使用描述子的方法相比(如ORB-SLAM[5])，這種方式的運算速度更快，且光流提供了合理的特徵點移動量；然而，它也缺乏特徵描述子可用於位置識别的能力。在VINS-Mono中，閉環(Loop closure)是可選用的，但若啟用閉環偵測會需要消耗大量運算資源，來從角點特徵中計算描述子。由於這個描述子僅用於閉環偵測，為求計算效能，本文介紹的方法並不會使用到描述子及閉環偵測。
了解了三個感測器的模型後，在進行最佳化前，我們需要初始化里程計，包含求得視覺尺度、重力方向以及初始速度。VINS-Mono是由影像算出SfM，並與IMU的資訊做鬆耦合融合(loosely-coupled fusion)。應用在車輛時，我們要確保系統尺度可以觀測，因此我們先以SfM 與輪速資訊算出尺度：

(4)
其中s為尺度，為相機在tk時的位姿。然後在固定尺度下，計算系統的初狀態
，v為初始速度，g為重力，計算式如下:

 (5) (6) (7)

為經由IMU和SfM所得的資訊所組成的測量值[3]，則代表初始狀態XI'與之間的轉換，與VINS-Mono不同的是我們的尺度已由(4)式求得。(pcb, Rcb)為相機到IMU間的位移量和旋轉量，可由外參校正工具[6]求得。及  為 IMU預積分之結果，代表透過IMU感測器所算出在[tk , tk+1]這段時間中的位移量及速度變化量。系統將會檢查計算出的重力值是否合理，若合理則初始化成功。