[發(fā)明專利]基于雙目視覺與IMU融合的SLAM方法

在審

• 申請?zhí)枺?/td>: 202010422434.6
• 申請日: 2020-05-19
• 公開（公告）號: CN111340851A
• 公開（公告）日: 2020-06-26
• 發(fā)明（設計）人: 劉繼廷;其他發(fā)明人請求不公開姓名
• 申請（專利權）人: 北京數(shù)字綠土科技有限公司;深圳綠土智能科技有限公司
• 主分類號: G06T7/254
• 分類號: G06T7/254;G06T7/285;G06T7/73;G06T7/80;G06K9/62
• 代理公司: 北京繪聚高科知識產(chǎn)權代理事務所(普通合伙) 11832
• 代理人: 羅碩

本發(fā)明實施例公開了一種基于雙目視覺與IMU融合的SLAM方法，該方法包括：根據(jù)雙目相機與IMU之間剛性約束，對IMU進行初始化，具體包括對IMU的角速度的偏置進行初始化以及對IMU的速度、重力進行初始化；構建包含有IMU殘差和相機重投影誤差的目標函數(shù)。相對于現(xiàn)有技術而言，本發(fā)明的技術方案可實現(xiàn)雙目視覺與IMU的融合，同時增加了系統(tǒng)的魯棒性。

技術領域

本發(fā)明涉及導航勘測技術領域，尤其涉及一種基于雙目視覺與IMU融合的SLAM方法。

背景技術

相機其能提供豐富的圖像信息，在移動SLAM領域同時定位與地圖創(chuàng)建得到了廣泛的應用。

但當移動機器人運動過快時，相機會出現(xiàn)運動模糊，或者幀與幀之間的重疊區(qū)域太少以至于無法進行特征匹配的問題；相比于相機，慣性測量單元IMU可以測得角速度和加速度，對于短時間的快速運動，慣性測量單元IMU能提供較好的運動估計與測量，另外慣性測量單元IMU會存在漂移，而相機的數(shù)據(jù)基本不會有漂移。

為了解決上述問題，研發(fā)人員提出了一種有效的解決方法就是融合視覺和IMU的信息，慣性測量單元IMU為快速運動提供了較好的解決方式，而相機又能在慢速運動下解決IMU的漂移問題。因此說如何實現(xiàn)視覺與IMU兩者融合至關重要。

目前現(xiàn)有的技術主要存在以下不足：1、例如：采用松耦合的方法進行融合：即將IMU和相機分別進行自身的運動估計，然后對其位姿估計結果進行融合。該方法能有效的改善系統(tǒng)的魯棒性和準確性，并且計算量小，但是這種方式并沒有考慮兩種傳感器（即IMU傳感器和視覺圖像傳感器）信息的內在關系，并且缺乏有效的約束和補償。2、例如：采用緊耦合的方法進行融合：即將IMU和相機的狀態(tài)合并在一起，共同構建運動方程和狀態(tài)方程，然后進行狀態(tài)估計。該方法充分的利用了傳感器的信息，提高了系統(tǒng)的魯棒性和準確性，但是計算量特別大。

發(fā)明內容

針對以上問題，本發(fā)明提出了一種基于雙目相機和IMU融合的SLAM方法。

本發(fā)明的主要技術目的在于，通過構建IMU的殘差與視覺的重投影誤差方程，并通過優(yōu)化的方法求解機器人的位姿，同時構建周圍環(huán)境的地圖。即具體的，初始化和構建構建IMU的殘差與視覺的重投影誤差方程；最終目的是求解特征點的三維信息，同時還有位姿信息，相機和IMU得外參；

有鑒于此，本發(fā)明實施例提出一種基于雙目視覺與IMU融合的SLAM方法。

本發(fā)明提出一種基于雙目視覺與IMU融合的SLAM方法，包括如下操作步驟：

根據(jù)雙目相機與IMU之間剛性約束，對IMU進行初始化，具體包括對IMU的陀螺儀的偏置進行初始化以及對IMU的速度、重力進行初始化；

構建包含有IMU殘差和相機重投影誤差的目標函數(shù)。

優(yōu)選的，作為一種可實施方案；基于雙目相機與IMU之間的剛性約束條件，對IMU的陀螺儀的偏置進行初始化，具體包括如下操作步驟：

初始化計算公式如下：

；第1公式；

其中，表示第j幀圖片在C₀坐標系的相對旋轉；為第i幀圖片在C₀坐標系的相對旋轉；上述第二項與第一項進行廣義乘法就得到了，所述為相機在第i幀到第j幀的相對旋轉；為陀螺儀從第j時刻到第i時刻的相對旋轉；C₀就是參考坐標系；

通過上述初始化計算公式計算得結果就是陀螺儀的初始偏置，具體實施時還包括如下計算步驟；

對上述陀螺儀從第j時刻到第i時刻的相對旋轉，進行一階近似計算得到第2公式；

；第2公式；