基于關鍵平面的視覺慣性里程計方法

本發明屬于slam(simultaneouslocalizationandmapping，同步定位與建圖)領域，具體涉及一種基于關鍵平面的視覺慣性里程計方法。

背景技術：

傳統的基于網格提取平面的視覺慣性里程計(visual-inertialodometry，vio)方法對每一幀圖像使用2d德勞內三角化形成2d網格，當特征點被三角化后生成3d網格，隨后建立直方圖，超過閾值的平面被篩選出當做面特征。將特征點和線分配到檢測到的每個平面，并且在后端優化面特征和點線特征。

這種方法雖然考慮到了特征點線和檢測到平面之間的限制，但是沒有考慮到平面和平面之間的限制，例如平行和垂直。

技術實現要素：

針對現有vio技術中的不足與難題，本發明旨在提供一種基于關鍵平面的視覺慣性里程計方法，在后端中增加平面和平面的限制，提高定位精度。

本發明通過以下技術方案予以實現：

基于關鍵平面的視覺慣性里程計方法，該方法包括以下步驟：

步驟1、單目相機以一定頻率采集圖像；imu以一定頻率采集慣性數據；

步驟2、對采集到的每幀圖像，提取點線特征并且進行跟蹤；

步驟3、imu預積分：對連續兩幀圖像之間的imu數據進行預積分；

步驟4、2d德勞內三角化；生成3d網格；檢測平面；對每個檢測到平面分配特征；判斷是否是關鍵平面。

步驟5、緊耦合視覺慣性定位：根據殘差解決最小二乘問題并得出最有估計，實現對目標載體的定位。

進一步地，步驟2具體如下：

步驟2.1、對圖像提取特征點，并在后一幀中建立與這些特征的關系，形成跟蹤；

步驟2.2、對圖像提取特征線，并在后一幀中建立與這些特征的關系，形成跟蹤。

進一步地，步驟3具體為：對任意兩幀圖像之間的imu數據，進行預積分，得到三個方向上的積分量；若imu的零偏改變，則可以直接用現有的預積分來更新。

進一步地，步驟4具體如下：

步驟4.1、2d德勞內三角化：對每一幀圖像，根據提取到的點特征和線特征，進行2d德勞內三角化，形成的網格屬于2d網格；

步驟4.2、生成3d的網格：對所有2d網格，如果網格中的三角形的三個頂點都被三角化，則計算三個頂點的3d坐標，并且將這個2d三角形變為3d三角面片，許多個三角面片組成3d網格；

步驟4.3、檢測平面：對每個3d三角面片，收集他們頂點的三維坐標，隨后建立直方圖提取水平平面和垂直平面；

步驟4.4、對每個檢測到平面分配特征，對每一個已經三角化的特征進行遍歷，若它與任何一個平面距離較近，則把該特征分配給平面，將特征-平面殘差添加到優化目標函數；

步驟4.5、判斷平面是否為關鍵平面：每次優化后會再進行一次特征分配給平面的過程。若分配給某個平面的特征數量小于一定數值，那么它就被剔除；剔除的時候會進行判斷，如果該平面連續一定數量的幀沒有被剔除，并且剔除時的幀和被檢測時候的幀的視差超過一定閾值，那么將其視為關鍵平面，將平面-關鍵平面殘差添加到優化目標函數。

進一步地，步驟5中的緊耦合視覺慣性定位具體為：將邊緣化先驗信息，imu預積分殘差，點和線重投影誤差，特征-平面殘差，平面-關鍵平面殘差加入目標優化函數進行優化。

與現有技術相比，本發明有益效果包括：

(1)本發明所提出的方法用于視覺慣性定位，所述方法同步視覺與imu信息，提出了篩選關鍵平面的策略，通過引入關鍵平面，增加探測到平面之間的規律性，提高了定位精度。

(2)使用本發明提出的方法，相較于傳統的僅僅基于特征點的視覺慣性定位系統，定位精度可提高約16％。

附圖說明

圖1是本發明基于關鍵平面視覺慣性里程計流程圖；

圖2是本發明3d網格建圖示意圖；

圖3是本發明采取關鍵平面策略建立的光滑平面；

圖4是沒采取關鍵平面策略建立的粗糙平面；

圖5是本發明2d德勞內三角化示意圖。

具體實施方式

下面結合附圖，對本發明作進一步地說明。

本技術領域技術人員可以理解的是，除非另外定義，這里使用的所有術語(包括技術術語和科學術語)具有與本發明所屬領域中的普通技術人員的一般理解相同的意義。還應該理解的是，諸如通用字典中定義的那些術語應該被理解為具有與現有技術的上下文中的意義一致的意義，并且除非像這里一樣定義，不會用理想化或過于正式的含義來解釋。

如圖1，一種基于關鍵平面的視覺慣性里程計方法，具體包括下述步驟：

步驟s1、通過雙目相機采集圖像，其分辨率為752*480，頻率為20hz，實驗中只采用左目圖像；通過imu測量角速度和加速度，頻率為100hz。

步驟s2、對每幀圖像提取特征并且追蹤這些特征；

s2a，對每幀圖像提取角點，并且通過lucas光流法在下一幀圖像確定角點的位置進行跟蹤，篩選角點的時候用基于基礎矩陣的ransac(randomsamplingconsistency，隨機采樣一致性)來剔除離群值；

s2b，對每幀圖像提取線特征并追蹤，使用lsd(linesegmentdetector，線段檢測器)提取線特征，并在每個線特征提取lbd(linebinarydescriptor，線二進制描述子)描述子來確定在下一幀中的位置。

步驟s3、imu預積分：根據兩幀圖像之間的所有慣性數據做預積分，這里的預積分給估計器的imu殘差使用：

imu測量模型如下：

式(1)中，at和ωt分別為t時刻imu坐標系下加速度真實值和角速度真實值，am和ωm分別為t時刻加速度與角速度測量值，和分別為t時刻加速度計與陀螺儀零偏，g^w是世界坐標系下的重力加速度，rtw為世界坐標系到t時刻imu坐標系的旋轉矩陣，na和nω分別為加速度計與陀螺儀的高斯噪聲。

根據imu測量模型，計算預積分測量值：

其中：

式(2)、(3)中，和表示第i個imu數據的三個預積分項，δt為兩個慣性數據之間的時間差，表示i時刻imu坐標系到第k個imu坐標系的旋轉矩陣，和分別表示i時刻加速度與角速度的測量值，表示四元數乘法。

步驟s4、2d德勞內三角化；生成3d網格；檢測平面；對每個檢測到平面分配特征；判斷是否是關鍵平面：

s4a，2d德勞內三角化：對每一幀圖像，提取點特征和線特征后，采取2d限制德勞內三角化；圖像中不僅含有點特征的信息還有線特征的信息，因此在構建2d網格的時候還有考慮圖像中的線特征。

s4b，生成3d網格：首先將2d德勞內限制三角化后得到的網格的每個三角形剖分下來，對于每個三角形，如果求出三個頂點的位置，那么三角形的3d坐標也能夠得到，經過對特征的三角化后，點線特征的3d位置有機會得到；如果某個三角形的三個頂點的三維坐標都已知，我們將該三角形的三維坐標求出，很多個三角面片組成一個3d網格；同時并不是每個3d網格都能被使用，本方法設置了兩個條件，兩個條件都滿足時該三角面片才能被使用，條件(1)為三角面片中最小的角度不得超過十五度；條件(2)為三角面片至少有兩個相鄰的三角面片。

s4c，檢測平面：這里只檢測水平和垂直平面；對于水平平面，根據imu提供的重力加速度方向，找出法向量近似與重力加速度的三角面片，隨后將這些三角面片的頂點高度提取出來，建立一維直方圖，坐標為頂點高度，超過閾值的量被選為水平平面；對于垂直平面，根據imu提供的重力加速度方向，找出法向量近似與重力加速度垂直的三角面片，隨后將這些三角面片的投影到xy平面，提取出軸角和離原點的距離，建立二維直方圖，其中一維坐標為軸角，另一維是離遠點的距離，超過閾值的被選為垂直平面。

s4d，對每個檢測到的平面分配特征：對滑動窗口的所有的點特征和線特征遍歷，若該特征到檢測平面的距離低于閾值，則將該特征認定為該平面所屬，同時將特征-平面殘差添加進入估計器。

s4e，判斷是否是關鍵平面：對每個面特征，我們實行剔除準則：若該平面擁有的點線特征數量低于閾值，則將該面特征刪除；刪除之后，實行關鍵幀篩選準側，(1)該平面連續一定數量的幀沒有被剔除；(2)剔除時的幀和被檢測時候的幀的視差超過一定閾值，如果上述兩個條件都滿足，那么將其視為關鍵平面，將平面-關鍵平面殘差添加到優化目標函數。

步驟s5、緊耦合視覺慣性定位：根據殘差解決最小二乘問題并得出最有估計，實現對目標載體的定位；

對滑動窗口所有待估計的狀態變量變量如下：

式(4)中，xk為imu坐標系k時刻的狀態，包括該時刻imu坐標系到世界坐標系的平移、旋轉、在世界坐標系下的速度以及零偏；λi表示第i個點特征在第一次被觀察到時相機坐標系的逆深度；oi為第i個線特征在世界坐標系下的正交坐標，k表示從0到n的正整數；πi＝[ni，di]表示第i個平面的坐標，其中ni為該平面的法向量，di為該平面到原點的距離；

類似的，πki＝[nki，dki]表示第i個平面的坐標，其中nki為該平面的法向量，dki為該平面到原點的距離。

最小化以下函數求解狀態量：

式(5)中，為bk和bk+1imu坐標系之間預積分的測量值；和分別表示在第j個相機坐標系下被觀測到的第i個點特征和線特征的觀測值；b表示窗口中所有預積分項的集合，f和g分別表示被相機觀測到的點特征和線特征的集合；h和j分別表示特征-平面以及平面-關鍵平面的殘差集合。

對應的殘差表示如下：

式(6)中，表示四元數的三維誤差量，[.]vec表示取四元數的虛部。

式(7)中，和分別表示第k個特征點在第i個和第j個相機坐標系下的觀測值；b1和b2為正切空間中觀測值向量任意選擇的一組基。

式(8)中，d(s，l)為點s到線l的距離，和為觀測值線段的兩個端點，l＝(l1，l2，l3)^t。

式(9)中，λk為該特征點在第一幀觀測zj中的逆深度，nj和dj分別為平面的法向量和離原點的距離。

關于平面-關鍵平面殘差，可以分為兩種情況：

(1)關鍵平面為水平平面

式(10)中，nj和nki表示平面與關鍵平面的法向量，dj和dki表示平面與關鍵平面到原點的距離。

(2)關鍵平面為垂直平面

式(11)中，nj和nki表示平面與關鍵平面的法向量，dj和dki表示平面與關鍵平面到原點的距離，g表示重力加速度。

本發明的軟件具有四個線程：點特征提取與跟蹤、線特征提取與跟蹤，網格生成與平面提取，估計器優化；四個線程并行運行，實現實時地對相機和imu數據處理。

使用開源數據集euroc進行算法評估，該數據集在一個工廠和室內環境采集慣性和視覺數據，工廠和室內環境有豐富的面結構，以使發明的算法提高性能。其圖像采集頻率為20hz，imu采樣頻率為200hz，數據集提供真實軌跡。本實例使用的硬件處理為8g的intelcorei5-8300筆記本，軟件平臺為ros。本方法(表格中記為proposed)實驗結果與基于特征點的視覺慣性里程計算法結果作比較。

圖2為發明算法建立的網格地圖，綠色的表示水平平面，如地板；紅色代表垂直平面，如墻壁，地板和墻壁在空間中的分布符合實際規律互相垂直，這得益于本發明的關鍵平面算法。

圖3為基于本發明關鍵平面算法得到的墻壁網格地圖，對比圖四表面更加光滑。

圖4為基于僅點特征的視覺慣性里程計算法得到的墻壁網格地圖，對比圖四表面更加粗糙。

圖5展示了2d德勞內三角化，紅點代表點特征，藍線段代表線特征，綠色網格代表經過限制德勞內三角化得到的2d網格。

最后給出了euroc序列在本方法和基于點特征兩種方法的均方根定位誤差(單位為厘米)定位結果，由表1可知本發明定位精度由于現有算法，定位平均精度提高了約16％。

表1

以上所述僅表達了本發明的優選實施方式，其描述較為具體和詳細，但并不能因此而理解為對本發明專利范圍的限制。應當指出的是，對于本領域的普通技術人員來說，在不脫離本發明構思的前提下，還可以做出若干變形、改進及替代，這些都屬于本發明的保護范圍。因此，本發明專利的保護范圍應以所附權利要求為準。