[發明專利]一種基于點線特征的視覺-慣性里程計狀態估計方法有效
| 申請號： | 202011401030.5 | 申請日： | 2020-12-02 |
| 公開（公告）號： | CN112683305B | 公開（公告）日： | 2022-03-04 |
| 發明（設計）人： | 史殿習;劉哲;楊文婧;楊紹武;徐利洋;崔玉寧;楊思寧;李林 | 申請（專利權）人： | 中國人民解放軍國防科技大學 |
| 主分類號： | G01C25/00 | 分類號： | G01C25/00 |
| 代理公司： | 湖南企企衛知識產權代理有限公司 43257 | 代理人： | 任合明 |
| 地址： | 410073 湖*** | 國省代碼： | 湖南;43 |
| 權利要求書： | 查看更多 | 說明書： | 查看更多 |
| 摘要： | |||
| 搜索關鍵詞： | 一種 基于 點線 特征 視覺 慣性 里程計 狀態 估計 方法 | ||
1.一種基于點線特征的視覺-慣性里程計狀態估計方法，其特征在于包括以下步驟：
第一步，構建基于特征點和特征線的視覺-慣性里程計系統；基于特征點和特征線的視覺-慣性里程計系統由數據獲取模塊、視覺特征追蹤模塊、IMU預積分模塊、特征重建模塊、非線性優化模塊、閉環檢測與重定位模塊組成；
數據獲取模塊與視覺特征追蹤模塊、IMU預積分模塊相連；數據獲取模塊從視覺-慣性里程計數據集下載數據，獲取雙目圖像幀中左目相機圖像幀和IMU數據，將雙目圖像幀中左目相機圖像幀作為單目相機圖像幀，將單目相機圖像幀發送給視覺特征追蹤模塊，將IMU數據發送給IMU預積分模塊；
視覺特征追蹤模塊與數據獲取模塊、特征重建模塊、閉環檢測與重定位模塊相連；視覺特征追蹤模塊從數據獲取模塊接收單目相機圖像幀，從單目相機圖像幀中提取特征點和特征線信息，對連續單目相機圖像幀中的特征進行匹配和追蹤，將特征點和特征線信息以及特征追蹤信息發送給特征重建模塊，將特征點信息發送給閉環檢測與重定位模塊；
IMU預積分模塊與數據獲取模塊、非線性優化模塊相連；IMU預積分模塊從數據獲取模塊接收IMU數據，利用IMU預積分算法對連續兩幀單目相機圖像幀中的IMU數據進行預積分處理，將預積分處理后的IMU預積分數據發送給非線性優化模塊；
特征重建模塊與視覺特征追蹤模塊、非線性優化模塊相連；特征重建模塊從視覺特征追蹤模塊獲取特征點和特征線信息以及特征追蹤信息，分別構建特征點管理和特征線管理數據結構，對第一次出現的特征進行三維重建，更新特征點管理和特征線管理數據結構，將更新后的特征點管理和特征線管理數據結構發送給非線性優化模塊；
閉環檢測與重定位模塊與視覺特征追蹤模塊、非線性優化模塊相連；閉環檢測與重定位模塊從視覺特征追蹤模塊獲取每一幀單目相機圖像幀中的特征點信息并進行儲存，通過特征點信息匹配，對每一幀單目相機圖像幀中的特征點信息進行閉環檢測，將閉環檢測結果發送給非線性優化模塊；
非線性優化模塊與特征重建模塊、IMU預積分模塊、閉環檢測與重定位模塊相連；非線性優化模塊從特征重建模塊獲取最新的特征點管理數據結構和特征線管理數據結構，從IMU預積分模塊獲取最新兩幀圖像間的IMU預積分數據，從閉環檢測與重定位模塊獲取閉環檢測結果，基于特征點管理數據結構和特征線管理數據結構、IMU預積分數據、系統狀態估計歷史數據、閉環檢測結果構建非線性優化損失函數并進行求解，獲取機器人狀態；
第二步，令變量k＝1，數據獲取模塊從視覺-慣性里程計數據集中獲取雙目圖像幀中左目相機圖像幀和IMU數據，將雙目圖像幀中左目相機圖像幀作為單目相機圖像幀；令數據獲取模塊接收到第一幀左目相機圖像幀時的時刻為t0時刻；
2.1，t0時刻數據獲取模塊將第一幀左目相機圖像幀發送給視覺特征追蹤模塊；
2.2，視覺特征追蹤模塊從數據獲取模塊得到第一幀左目相機圖像幀作為第一幀單目相機圖像幀，視覺特征追蹤模塊利用FAST算法提取第一幀單目相機圖像幀中的MAXpoint個特征點，利用EDLines算法提取第一幀單目相機圖像幀中MAXline條特征線，根據特征點坐標與特征線坐標關系獲取第一幀單目相機圖像幀中特征線上特征點信息，并將特征點和特征線信息發送至特征重建模塊，將特征點信息發送至閉環檢測與重定位模塊，MAXpoint、MAXline均為正整數；
2.3，特征重建模塊從視覺特征追蹤模塊得到第一幀單目相機圖像幀中的特征點信息與特征線信息，特征重建模塊構建第一幀特征點管理數據結構實例，該實例包括第一幀單目相機圖像中每個特征點的圖像坐標系坐標、每個特征點的相機坐標系坐標，特征重建模塊構建第一幀特征線管理數據結構實例，該實例包括第一幀單目相機圖像中每條特征線段起始點的圖像坐標系坐標、每條特征線段起始點的相機坐標系坐標、每條特征線段上的特征點圖像坐標系坐標、每條特征線段上所有特征點相機坐標系坐標集合；
2.4，閉環檢測與重定位模塊從視覺特征追蹤模塊得到第一幀單目相機圖像幀中的特征點信息，采用DBoW2算法對第一幀單目相機圖像中的特征點信息進行轉化、存儲；
第三步，數據獲取模塊從視覺-慣性里程計數據集中獲取雙目圖像幀中左目相機圖像幀和IMU數據，將雙目圖像幀中左目相機圖像幀作為單目相機圖像幀；數據獲取模塊在tk時刻從視覺-慣性里程計數據集中接收到第k+1幀單目相機圖像幀；數據獲取模塊將第k+1幀單目相機圖像幀發送給視覺特征追蹤模塊，將[tk-1,tk]時間區間內從視覺-慣性里程計數據集中接收到IMU數據發送給IMU預積分模塊；
第四步，視覺特征追蹤模塊對第k+1幀單目相機圖像進行特征提取與特征追蹤，方法是：
4.1，視覺特征追蹤模塊利用KLT稀疏光流算法，在第k+1幀單目相機圖像中跟蹤第k幀單目相機圖像中的特征點，得到第k+1幀單目相機圖像中的特征點集合Pk+1，Pk+1中的元素為特征點，每個特征點由其在第k+1幀單目相機圖像中的坐標表示；
4.2，視覺特征追蹤模塊利用EDLines算法提取第k+1幀單目相機圖像中的MAXline條特征線段，得到第k+1幀單目相機圖像中的特征線段集合Lk+1，Lk+1中的元素為特征線段la，a＝1,2,…,A，A表示Lk+1中特征線段的總數，la表示為la(sa，ea)，sa為特征線段la的起點，ea為特征線段la的終點，表示sa在單目圖像坐標系的x軸坐標，表示sa在單目圖像坐標系的y軸坐標；
4.3，視覺特征追蹤模塊對Lk+1中的特征線段進行整合，刪除誤檢測線段，合并屬于同一條直線的多條線段，得到整合后的新特征線段集合Lk+1；
4.4，視覺追蹤模塊根據特征點與特征線關系，對第k+1幀單目相機圖像中的特征點進行增補，得到新的特征點集合Pk+1；
4.5，視覺追蹤模塊根據特征點與特征線關系，對第k+1幀單目相機圖像中的特征線段和第k幀單目相機圖像中的特征線段進行匹配，以實現單目相機圖像幀間特征線的追蹤，方法是：
4.5.1，對Lk+1中的特征線段進行篩選，記錄Lk+1中特征線與Pk+1中特征點之間的關系即每條特征線段上有哪些特征點，將Lk+1中特征線段上特征點數小于2的特征線段從Lk+1中剔除，對Lk+1中的特征線段由長至短重新排序，更新Lk+1；
4.5.2，對重新排序后的Lk+1中的線段進行編號，令線段lc∈Lk+1，c＝1,2,…,C，C表示Lk+1中線段的總數，lc可表示為lc(sc，ec)，sc為線段lc的起點，ec為線段lc的終點，表示sc在單目圖像坐標系x軸坐標，表示sc在單目圖像坐標系y軸坐標；
4.5.3，選取Lk+1中的特征線段lc，在第k幀單目相機圖像特征線段集合Lk中依次尋找與lc相匹配的特征線段，即屬于不同圖像幀的同一條特征線段：
4.5.3.1，選取Lk中的特征線段ld，ld∈Lk，d＝1,2,…,D，D表示Lk中線段的總數，若交換ld起點與終點，即令ld(sd，ed)＝(ed，sd)，轉4.5.3.2；否則，直接轉4.5.3.2；
4.5.3.2，若lc與ld擁有相同的特征點數超過2個，lc與ld線段長度差值小于閾值Tl，Tl大小為30個像素，且lc起點sc到ld起點sd距離小于閾值Tpp，Tpp大小為60個像素，且lc終點ec到ld終點ed距離小于閾值Tpp，則lc與ld匹配成功；轉4.5.4；若上述條件有一項不滿足，則lc與ld匹配不成功，轉4.5.5；
4.5.4，此時lc與ld匹配成功，表示第k幀單目圖像中的特征線段ld在第k+1幀單目圖像中追蹤成功，記錄匹配與追蹤信息，包括lc與ld在Lk+1與Lk中的序號，若cC，令c＝c+1，轉4.5.3；若c＝C，轉4.5.6；
4.5.5，此時lc與ld匹配不成功，若dD，令d＝d+1，轉4.5.3.1；若d＝D且cC，令c＝c+1，轉4.5.3；若d＝D且c＝C，轉4.5.6；
4.5.6，得到特征線匹配后第k+1幀單目相機圖像與第k幀單目相機圖像中特征線段的追蹤匹配信息，即第k+1幀單目相機圖像中特征線段與第k幀單目相機圖像中特征線段之間的對應關系；
4.6，視覺追蹤模塊將第k+1幀單目相機圖像中的特征點信息、特征線信息傳送至特征重建模塊，將第k+1幀單目相機圖像與第k幀單目相機圖像中的特征點、特征線追蹤匹配信息傳送至特征重建模塊，將第k+1幀單目相機圖像中的特征點信息傳送至閉環檢測與重定位模塊；
第五步，IMU預積分模塊對從數據獲取模塊得到的[tk-1,tk]時間區間內的IMU數據，采用預積分算法進行預積分處理，將預積分處理后的[tk-1,tk]時間區間內的IMU預積分值發送至非線性優化模塊；
第六步，特征重建模塊從視覺特征追蹤模塊獲取第k+1幀單目相機圖像中的特征點信息、特征線信息，以及第k幀單目相機圖像與第k+1幀單目相機圖像中的特征點、特征線追蹤匹配信息，根據以上信息分別構建第k+1幀特征點管理和特征線管理數據結構實例，對特征進行三維重建，更新特征點管理和特征線管理數據結構，將更新過的特征點管理和特征線管理數據結構發送給非線性優化模塊，方法如下：
6.1，特征重建模塊根據第k+1幀單目相機圖像中的特征點信息和第k幀單目相機圖像與第k+1幀單目相機圖像中的特征點追蹤匹配信息，構建第k+1幀特征點管理數據結構實例，該實例包括第k+1幀單目相機圖像中每個特征點的圖像坐標系坐標、每個特征點的相機坐標系坐標；
6.2，特征重建模塊根據第k+1幀單目相機圖像中的特征線信息和第k幀單目相機圖像與第k+1幀單目相機圖像中的特征線追蹤匹配信息，構建第k+1幀特征線管理數據結構實例，該實例包括第k+1幀單目相機圖像中每條特征線段起始點的圖像坐標系坐標、每條特征線段起始點的相機坐標系坐標、每條特征線段上的特征點圖像坐標系坐標、每條特征線段上所有特征點相機坐標系坐標集合；
6.3，若k4，特征檢測模塊將此時的第k+1幀特征點管理數據結構和特征線管理數據結構發送給非線性優化模塊，轉第七步；
6.4，若k≥4，特征檢測模塊對點特征和線特征進行三維重建，方法是：
6.4.1，特征重建模塊對于第k+1幀單目相機圖像中的特征點，篩選出至少在第k幀中出現過的特征點，聯立這些特征點在第k+1、k、k-1、k-2、k-3幀中的相機坐標系坐標信息，利用奇異值分解算法，對這些特征點在相機坐標系中的深度進行求解，并在第k+1幀單目相機圖像特征點管理數據結構實例中添加這些特征點的相機坐標系深度；
6.4.2，特征重建模塊對于第k+1幀單目相機圖像中的特征線，篩選出至少在第k幀中出現過的特征線lf，lf∈LF，LF表示第k+1幀單目相機圖像中所有被篩選出來的特征線的集合，基于lf上特征點信息，進行三維重建；
6.4.3，特征重建模塊將更新過的第k+1幀特征點管理數據結構和特征線管理數據結構發送給非線性優化模塊，轉第七步；
第七步，閉環檢測與重定位模塊從視覺特征追蹤模塊獲取第k+1幀單目相機圖像中的特征點信息，并基于DBoW2算法對第k+1幀單目相機圖像中的特征點信息進行轉化、存儲與檢索，并利用VINS-Mono中的閉環檢測算法，對第k+1幀單目相機圖像幀與之前的單目圖像幀進行閉環檢測匹配，若匹配成功，閉環檢測與重定位模塊將閉環檢測結果發送給非線性優化模塊，轉第八步；若匹配不成功，直接轉第八步；
第八步，非線性優化模塊從特征重建模塊獲取第k+1幀特征點管理和特征線管理數據結構，從IMU預積分模塊獲取[tk-1,tk]時間區間內的IMU預積分數據，從閉環檢測與重定位模塊獲取閉環檢測結果，若k9，直接轉第九步；若k≥9，非線性優化模塊基于特征點數據、特征線數據、IMU預積分數據、系統狀態估計歷史數據、閉環檢測數據構建非線性優化損失函數并進行求解，獲取機器人狀態，方法是：
8.1，非線性優化模塊構造滑動窗口，窗口內包括{tk-9,tk-8,…,tk-1,tk}共十個時刻內機器人系統相關信息，機器人系統相關信息包括從特征重建模塊接收到的第k-8,k-7,…,k+1共十幀圖像幀特征點管理和特征線管理數據結構、從IMU預積分模塊中接收到[tk-10,tk-9],[tk-9,tk-8],…,[tk-1,tk]共十個時間區間內的IMU預積分信息、從閉環檢測與重定位模塊中接收到的閉環檢測信息、除tk時刻外每個時刻機器人的位姿信息；
8.2，非線性優化模塊將十個時刻中所有特征線段的普呂克坐標參數轉化為線段所在三維直線的正交法表達參數；
8.3，非線性優化模塊根據滑動窗口的信息，構造優化變量χ：
其中，xk-9,xk-8,…,xk表示滑動窗口內十個時刻機器人系統狀態，λ0,λ1,…,λn表示滑動窗口內十個時刻中所有特征點在三維世界中的深度，表示滑動窗口內十個時刻中所有特征線段的正交法表達參數，ti時刻機器人系統狀態xi可以表達為：
其中，代表ti時刻機器人在IMU坐標系相對世界坐標系的位移向量，代表ti時刻機器人在IMU坐標系相對世界坐標系的速度向量，代表ti時刻機器人在IMU坐標系相對世界坐標系的四元數，代表ti時刻機器人在IMU坐標系中的IMU加速度計偏置，代表ti時刻機器人在IMU坐標系中的IMU陀螺儀偏置；
8.4，非線性優化模塊根據8.3中構造的優化變量構造優化損失函數：
其中：
表示IMU預積分殘差項，表示IMU坐標系中從ti時刻到ti+1時刻IMU預積分值，表示IMU預積分數據集合，表示IMU從ti時刻到ti+1時刻IMU預積分殘差，該殘差的具體構造方式同VINS-Mono；
表示特征點殘差項，表示相機坐標系中ti時刻第j個特征點的重投影誤差，j≤l，表示滑動窗口內所有特征點的集合，表示ti時刻第j個特征點殘差，該殘差的具體構造方式同VINS-Mono；
表示特征線殘差項，表示相機坐標系中ti時刻第m個特征點的重投影誤差，m≤n，表示滑動窗口內所有特征線的集合，表示ti時刻第m個特征點殘差，該殘差的具體構造方式同PL-VIO；
表示滑動窗口外系統狀態估計歷史數據殘差項，該殘差項的具體構造方式同VINS-Mono；
表示閉環檢測重定位殘差項，若閉環檢測發生在ti時刻與tv時刻之間，即tv時刻單目圖像幀和ti時刻單目圖像幀被判定為發生閉環檢測，tv時刻可為ti時刻之前任一時刻，vi，表示相機坐標系中ti時刻第j個特征點在tv時刻單目圖像幀上的重投影誤差，表示世界坐標系中tv時刻機器人的旋轉四元數，表示世界坐標系中tv時刻機器人的位移向量，下標w表示該數據為世界坐標系中的數據，表示閉環檢測模塊中存儲的圖像信息集合，表示滑動窗口內所有特征點的集合，表示ti時刻閉環檢測殘差，該殘差的具體構造方式同VINS-Mono；
8.5，非線性優化模塊采用最小二乘優化方法對優化損失函數求解，獲得tk時刻機器人位姿估計值、速度估計值、窗口內所有特征點深度估計值、窗口內所有特征線普呂克坐標參數估計值，轉第九步；
第九步，令k＝k+1，若還能從視覺-慣性里程計數據集中獲取雙目圖像幀中左目相機圖像幀和IMU數據，返回第三步，若不能從視覺-慣性里程計數據集中獲取雙目圖像幀中左目相機圖像幀和IMU數據，轉第十步；
第十步，結束。
該專利技術資料僅供研究查看技術是否侵權等信息，商用須獲得專利權人授權。該專利全部權利屬于中國人民解放軍國防科技大學，未經中國人民解放軍國防科技大學許可，擅自商用是侵權行為。如果您想購買此專利、獲得商業授權和技術合作，請聯系【客服】
本文鏈接：http://www.szxzyx.cn/pat/books/202011401030.5/1.html，轉載請聲明來源鉆瓜專利網。
