一種基于SINS/GPS和里程計輔助的視覺SLAM方法技術

本發明專利技術公開了一種基于SINS/GPS和里程計輔助的視覺SLAM方法，包括如下步驟：當全球衛星定位系統信號可用時，將GPS與捷聯慣性導航系統輸出信息進行數據融合，得到姿態、速度、位置等信息；當GPS信號不可用時，將里程計與SINS輸出信息進行數據融合，得到姿態、速度、位置等信息；利用雙目攝像機拍攝得到環境圖片，對其進行特征提取和特征匹配；利用上述得到的先驗姿態、速度、位置信息和環境特征實現定位與地圖構建，完成視覺SLAM算法。本發明專利技術利用SINS、GPS和里程計輔助視覺SLAM，能夠實現室外和室內兩種環境下的定位與地圖構建，應用范圍廣泛，且能夠提高定位的精度和魯棒性。

A visual SLAM method based on SINS/GPS and odometer aided

The invention discloses an auxiliary SINS/GPS and visual SLAM based on mileage method comprises the following steps: when the GPS signal is available, GPS and strapdown inertial navigation system output information for data fusion, get the attitude velocity and position information; when the GPS signal is not available, the odometry data the fusion and SINS output information, get posture, speed and position information; shooting environment images using the binocular camera, the feature extraction and feature matching; using the obtained prior attitude, velocity and position information and environmental characteristics of construction and map positioning, visual SLAM algorithm. The invention utilizes SINS, GPS and odometer to assist visual SLAM, and can realize the positioning and map construction in two outdoor environments and indoor environments, and has wide application range, and can improve the positioning accuracy and robustness.

【技術實現步驟摘要】
一種基于SINS/GPS和里程計輔助的視覺SLAM方法
本專利技術涉及移動機器人自動導航
，尤其是一種基于SINS/GPS和里程計輔助的視覺SLAM方法。
技術介紹
同時定位與地圖構建(Simultaneouslocalizationandmapping，SLAM)問題是移動機器人研究領域的基本問題之一，是移動機器人在未知環境中實現自主導航與自主控制的關鍵。解決SLAM問題的方法有很多，隨著視覺傳感器的發展、成本的降低以及圖像處理技術的提高，視覺SLAM方法得到了廣泛的應用，視覺傳感器也逐步取代其他傳感器稱為解決SLAM問題中獲取外部環境信息的主要傳感器。然而，隨著SLAM方法應用的廣泛化，其應用的環境也越來越多，單一的傳感器已經無法在復雜環境下解決SLAM問題。因此，出現了多傳感器協同解決SLAM問題。目前，應用比較多的是GPS輔助視覺SLAM方法或里程計輔助視覺SLAM方法。然而，GPS信號在有遮擋環境下或室內環境下很容易失效，而里程計的誤差較大，無法適應較復雜的環境。一旦一種傳感器失效，則定位導航的精度將受到很大的影響。因此，出現一種能夠適應多種環境、室內室外均可應用，且精度相對較高的SLAM方法是很有必要的。
技術實現思路
專利技術目的：為克服現有技術中存在的不足，本專利技術提供了一種可以適應多種環境且能夠提高定位、制圖精度的SLAM方法。技術方案：為解決上述技術問題，本專利技術提供了一種基于SINS/GPS和里程計輔助的視覺SLAM方法，包括如下步驟：步驟1：當GPS信號可用時，將GPS與SINS輸出信息進行數據融合，得到姿態、速度、位置信息；步驟2：當GPS信號不可用時，將里程計與SINS輸出信息進行數據融合，得到姿態、速度、位置信息；步驟3：利用雙目攝像機拍攝得到環境圖片，對環境圖片進行特征提取和特征匹配；步驟4：利用步驟1和步驟2得到的姿態、速度、位置信息和環境特征實現定位與地圖構建。進一步的，所述步驟1中當GPS信號可用時，將GPS與SINS輸出信息進行數據融合，得到姿態、速度、位置信息，具體為：步驟1.1：選擇導航坐標系n為地理坐標系，其xyz分被指向“東北天”方向；選擇載體坐標系b的原點在載體質心，其xyz分被指向載體的“右前上”；選取系統狀態向量為：其中，分別為東向和北向速度誤差；φu、φe、φn分別為天向、東向和北向平臺誤差角；δL、δλ、δh分別為載體緯度、經度和高度誤差；分別為b系下xyz三軸的陀螺常值誤差；分別為b系下xy方向上的加速度計常值誤差；SINS/GPS系統的狀態方程表示為：其中，φ＝[φuφeφn]T，RM、RN分被為卯酉圈和子午圈半徑，為計算出的速度，為計算出的速度誤差，為計算得到的數學平臺旋轉角速度，為地球旋轉角速度，為計算得到的數學平臺相對地球的旋轉角速度，為對應的計算誤差；分別為b系下陀螺儀和加速度計的隨機誤差；Cω、分別為：步驟1.2：選擇載體的速度作為觀測量：SINS/GPS系統的觀測方程表示為：其中，為載體SINS解算出的速度；為GPS輸出的速度；ν為零均值高斯白噪聲過程；Hk為觀測矩陣，且：Hk＝[I2×202×10]；步驟1.3：使用四階龍格-庫塔積分方法，以采樣周期T為步長將步驟1.1和步驟1.2分別建立的SINS/GPS系統的狀態方程和觀測方程離散化，得到離散形式的狀態方程和觀測方程，記為：利用容積卡爾曼濾波方法，對SINS/GPS系統進行數據融合，其過程如下：(1)選定濾波初值：(2)計算2n個sigma點與對應權值：(3)對k-1時刻狀態估計誤差方差陣Pk-1進行Cholesky分解：(4)求取2n個容積點并通過非線性狀態方程進行傳播：(5)求取狀態一步預測值與誤差方陣一步預測值：(6)計算量測預測值：(7)計算量測預測誤差方差陣：(8)計算狀態和量測間互協方差陣為(9)計算增益矩：(10)計算狀態值估計值與狀態估計誤差方差陣：估計出狀態向量后，通過閉環反饋，得到SINS/GPS系統輸出的位置、速度、姿態信息；當GPS無信號或者系統停止運行時數據融合結束，否則，(2)～(10)循環執行。進一步的，所述步驟2中當GPS信號不可用時，將里程計與SINS輸出信息進行數據融合，得到姿態、速度、位置信息，具體為：步驟2.1：選取SINS/OD系統狀態向量為：其中，δαψ為里程計航向安裝誤差角；δαθ為里程計俯仰安裝誤差角；δKD為里程計的標度誤差；分別為東向和北向速度誤差；φu、φe、φn分別為天向、東向和北向平臺誤差角；δL、δλ、δh分別為載體緯度、經度和高度誤差；分別為b系下xyz三軸的陀螺常值誤差；分別為b系下xy方向上的加速度計常值誤差；SINS/OD系統的狀態方程表示為：步驟2.2：選擇載體的速度作為觀測量：SINS/OD系統的觀測方程表示為：其中，為載體SINS解算出的速度；為里程計輸出的速度；為b系下的真實速度；為里程計在b系下輸出誤差向量；ν為零均值高斯白噪聲過程；δαψ和δαθ一般較小，近似表示為：其中，KD為里程計的標度系數；PD為里程計的輸出脈沖；步驟2.3：使用四階龍格-庫塔積分方法，以采樣周期T為步長將步驟2.1和步驟2.2分別建立的SINS/OD狀態方程和觀測方程離散化，得到離散形式的狀態方程和觀測方程，記為：利用容積卡爾曼濾波(CKF)方法，對SINS/OD系統進行數據融合，其過程如下：(1)選定濾波初值：(2)計算2n個sigma點與對應權值：(3)對k-1時刻狀態估計誤差方差陣Pk-1進行Cholesky分解：(4)求取2n個容積點并通過非線性狀態方程進行傳播：(5)求取狀態一步預測值與誤差方陣一步預測值：(6)將一步預測誤差方差Pk/k-1陣進行Cholesky分解：(7)求取2n個容積點并通過非線性量測方程進行傳播：(8)計算量測預測值：(9)計算量測預測誤差方差陣：(10)計算狀態和量測間互協方差陣為(11)計算增益矩：(12)計算狀態值估計值與狀態估計誤差方差陣：估計出狀態向量后，通過閉環反饋，得到SINS/OD系統輸出的位置、速度、姿態信息；當GPS有信號或者系統停止運行時數據融合結束，否則，(2)～(11)循環執行。進一步的，所述步驟3中利用攝像機拍攝得到環境圖片，對環境圖片進行特征提取和特征匹配，具體為：步驟3.1：由雙目攝像機拍攝得到左右兩張圖片；對每張圖片，利用高斯卷積核對圖像進行線性尺度變換，構建尺度空間：其中，I(x,y)為二維圖像，G(x,y,σ)為尺度可變的高斯函數，具體表達式如下：圖像尺度空間L(x,y,σ)是通過圖像I(x,y)與具有可變核的高斯濾波函數G(x,y,σ)進行卷積形成的；用圖像的高斯金子塔來表示，金子塔的層數為：log(width)/log(2)-2，width為圖像寬度；每層放置4張經不同高斯核卷積核處理過的圖像；步驟3.2：對同一層內兩張相鄰的經過高斯卷積的圖片做差，得到高斯差分金字塔(DOG)：在三維尺度空間D(x,y,σ)中，搜索每個點的26個領域，如果該點為局部極值點，則將該點保存為候選關鍵點；對D(x,y,σ)進行泰勒展開：其中：去除對比度低的關鍵點，即：的點；計算Hessian矩陣H：設矩陣H的兩個特征值分別為λ1和λ2，且：λ1＝rλ2；則：設置r本文檔來自技高網...

【技術保護點】
一種基于SINS/GPS和里程計輔助的視覺SLAM方法，其特征在于，包括如下步驟：步驟1：當GPS信號可用時，將GPS與SINS輸出信息進行數據融合，得到姿態、速度、位置信息；步驟2：當GPS信號不可用時，將里程計與SINS輸出信息進行數據融合，得到姿態、速度、位置信息；步驟3：利用雙目攝像機拍攝得到環境圖片，對環境圖片進行特征提取和特征匹配；步驟4：利用步驟1和步驟2得到的姿態、速度、位置信息和環境特征實現定位與地圖構建。

【技術特征摘要】
1.一種基于SINS/GPS和里程計輔助的視覺SLAM方法，其特征在于，包括如下步驟：步驟1：當GPS信號可用時，將GPS與SINS輸出信息進行數據融合，得到姿態、速度、位置信息；步驟2：當GPS信號不可用時，將里程計與SINS輸出信息進行數據融合，得到姿態、速度、位置信息；步驟3：利用雙目攝像機拍攝得到環境圖片，對環境圖片進行特征提取和特征匹配；步驟4：利用步驟1和步驟2得到的姿態、速度、位置信息和環境特征實現定位與地圖構建。2.如權利要求1所述的基于SINS/GPS和里程計輔助的視覺SLAM方法，其特征在于，所述步驟1中當GPS信號可用時，將GPS與SINS輸出信息進行數據融合，得到姿態、速度、位置信息，具體為：步驟1.1：選擇導航坐標系n為地理坐標系，其xyz分被指向“東北天”方向；選擇載體坐標系b的原點在載體質心，其xyz分被指向載體的“右前上”；選取系統狀態向量為：其中，分別為東向和北向速度誤差；φu、φe、φn分別為天向、東向和北向平臺誤差角；δL、δλ、δh分別為載體緯度、經度和高度誤差；分別為b系下xyz三軸的陀螺常值誤差；分別為b系下xy方向上的加速度計常值誤差；SINS/GPS系統的狀態方程表示為：其中，φ＝[φuφeφn]T，RM、RN分被為卯酉圈和子午圈半徑，為計算出的速度，為計算出的速度誤差，為計算得到的數學平臺旋轉角速度，為地球旋轉角速度，為計算得到的數學平臺相對地球的旋轉角速度，為對應的計算誤差；分別為b系下陀螺儀和加速度計的隨機誤差；Cω、分別為：步驟1.2：選擇載體的速度作為觀測量：SINS/GPS系統的觀測方程表示為：其中，為載體SINS解算出的速度；為GPS輸出的速度；ν為零均值高斯白噪聲過程；Hk為觀測矩陣，且：Hk＝[I2×202×10]；步驟1.3：使用四階龍格-庫塔積分方法，以采樣周期T為步長將步驟1.1和步驟1.2分別建立的SINS/GPS系統的狀態方程和觀測方程離散化，得到離散形式的狀態方程和觀測方程，記為：利用容積卡爾曼濾波方法，對SINS/GPS系統進行數據融合，其過程如下：(1)選定濾波初值：(2)計算2n個sigma點與對應權值：(3)對k-1時刻狀態估計誤差方差陣Pk-1進行Cholesky分解：(4)求取2n個容積點并通過非線性狀態方程進行傳播：(5)求取狀態一步預測值與誤差方陣一步預測值：(6)計算量測預測值：(7)計算量測預測誤差方差陣：(8)計算狀態和量測間互協方差陣為(9)計算增益矩：(10)計算狀態值估計值與狀態估計誤差方差陣：估計出狀態向量后，通過閉環反饋，得到SINS/GPS系統輸出的位置、速度、姿態信息；當GPS無信號或者系統停止運行時數據融合結束，否則，(2)～(10)循環執行。3.如權利要求1所述的基于SINS/GPS和里程計輔助的視覺SLAM方法，其特征在于，所述步驟2中當GPS信號不可用時，將里程計與SINS輸出信息進行數據融合，得到姿態、速度、位置信息，具體為：步驟2.1：選取SINS/OD系統狀態向量為：其中，δαψ為里程計航向安裝誤差角；δαθ為里程計俯仰安裝誤差角；δKD為里程計的標度誤差；分別為東向和北向速度誤差；φu、φe、φn分別為天向、東向和北向平臺誤差角；δL、δλ、δh分別為載體緯度、經度和高度誤差；分別為b系下xyz三軸的陀螺常值誤差；分別為b系下xy方向上的加速度計常值誤差；SINS/OD系統的狀態方程表示為：步驟2.2：選擇載體的速度作為觀測量：SINS/OD系統的觀測方程表示為：其中，為載體SINS解算出的速度；為里程計輸出的速度；為b系下的真實速度；為里程計在b系下輸出誤差向量；ν為零均值高斯白噪聲過程；δαψ和δαθ一般較小，近似表示為：其中，KD為里程計的標度系數；PD為里程計的輸出脈沖；步驟2.3：使用四階龍格-庫塔積分方法，以采樣周期T為步長將步驟2.1和步驟2.2分別建立的SINS/OD狀態方程和觀測方程離散化，得到離散形式的狀態方程和觀測方程，記...

【專利技術屬性】
技術研發人員：徐曉蘇，楊博，代維，
申請(專利權)人：東南大學，
類型：發明
國別省市：江蘇,32