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

在審

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

1.一種基于雙目視覺與IMU融合的SLAM方法，其特征在于，包括：

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

構建包含有IMU殘差和相機重投影誤差的目標函數；

基于雙目相機與IMU之間的剛性約束條件，對IMU的陀螺儀的偏置進行初始化，具體包括如下操作步驟：

初始化計算公式如下：

；第1公式；

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

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

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

；第2公式；

第2個公式為的一階線性近似，將其第2公式帶入到第1公式中，再根據第1公式中的第二項與第一項做差近似為零這個約束條件，求解得到陀螺儀的初始偏置；其中，該第2公式主要是對的解釋，為IMU從第j時刻到第i時刻的相對旋轉； 表示IMU的角度變化函數對角速度的偏置進行求偏導；為通過IMU的測量角速度所求的角度變化，通過對最小二乘函數進行最小化就可以得到IMU的角速度的初始偏置。

2.根據權利要求1所述的基于雙目視覺與IMU融合的SLAM方法，其特征在于，

基于雙目相機與IMU之間的剛性約束條件，對IMU的速度、重力進行初始化，具體包括如下操作步驟：

設定有關初始化的速度和重力加速度的目標求解值，即設定為目標求解值，其表達公式如下；；第3公式；

在第3公式中，為目標求解值；其中為第i幀圖片的速度；為第k幀圖片的速度；由于使用的是雙目相機，所以尺度因子為1；表示在C₀坐標系下的重力加速度，C₀就是參考坐標系；上述中括號里面的因子即為初始化速度和重力加速度；

列舉為第j幀到第i幀的位移變化公式；；第4公式；

列舉為第j幀到第i幀的角度變化公式；；第5公式；

在第4公式中，為第j幀到第i幀的位移變化；表示第j時刻到C₀坐標系的位置變化；表示第i時刻到C₀坐標系的位置變換；這個參數是第i時刻到C₀坐標的旋轉變換；為第幀圖片的速度；是第i幀到第j幀的時間差；

在第5公式中，為第j幀到第i幀的角度變化；為C₀坐標系到第i幀的相對旋轉；是第j時刻到C₀坐標的旋轉變換；是第i時刻到C₀坐標的旋轉變換；其中為第j幀圖片的速度；是第i幀到第j幀的時間差；表示在C₀坐標系下的重力加速度；這個參數是第i時刻到C₀坐標的旋轉變換；為第i幀圖片的速度；

列舉相機坐標系的平移信息轉換到IMU到初始坐標系的平移信息，計算公式如下：；第6公式；

在上述第6公式中，為第i時刻到C₀坐標系的位置變換；為第C_i幀到C₀坐標系的位置變換；為第C_i時刻到C₀坐標的旋轉變換；為相機到IMU的平移信息，該第6公式是將在相機坐標系的平移信息轉換到IMU到初始坐標系的平移信息；上述第6公式的主要作用是，用來做相機和IMU之間的坐標變換；

然后將上述第4公式、第5公式和第6公式合并構成目標方程，即第7公式，最后根據所述目標方程求解得到得到上述IMU的初始化速度和重力加速度；其中，這樣得到目標方程：

；第7公式；

上述第7公式中，通過求解這個目標方程可以得到為目標求解值的數值；即根據所述第7公式中目標方程，可以得到上述IMU的初始化速度和重力加速度。

3.根據權利要求2所述的基于雙目視覺與IMU融合的SLAM方法，其特征在于，

所述構建包含有IMU殘差和相機重投影誤差的目標函數，具體包括如下操作步驟：

確定IMU待優化的狀態量，設定有關待優化的狀態量的目標求解值，即設定為目標求解值：

；第8公式；

其中，上述第8公式的其只是一個待求解的目標值，換言之其只是一個標號公式而已；上述第8公式中為平移信息，為速度信息，為位姿信息，為IMU的加速度的偏置，為IMU的陀螺儀的偏置；

根據上述待優化的狀態量列舉目標函數方程為：

；第9公式；

其中，所述目標函數方程中的第一項為IMU的殘差；對上述第9公式中的第一項進行展開換算得到第10公式；

；第10公式；

在上述第10公式中，為位移差，為速度差，為角度差，為加速度計的偏置差，為陀螺儀的偏置差；為第i幀時刻在參考坐標系下旋轉矩陣的轉置；為參考坐標系下的第j幀圖片的速度；為參考坐標系下的重力加速度；為參考坐標系下的第i幀圖片的速度；為第j幀時刻在參考坐標系的平移信息；為第i幀時刻在參考坐標系的平移信息；為重力加速度；為時間差；為IMU預積分得到的位移；為IMU預積分得到速度；為IMU預積分得到得角度；表示第i幀時刻在參考坐標系下的姿態轉置；為第j幀時刻在參考坐標系下的姿態；為參考坐標系下的第j幀圖片的加速度偏置；為參考坐標系下的第i幀圖片的加速度偏置；為參考坐標系下的第j幀圖片的陀螺儀偏置；為參考坐標系下的第i幀圖片的陀螺儀偏置；

另外，在第10公式中，該第10公式的矩陣的內容則是根據，上述第10公式中的矩陣側的第一項可通過第4公式代入換算，且上述第10公式中的矩陣側的第二項可通過第5公式代入換算，其中上述第10公式中的矩陣側的第三項是兩個四元數之間的乘法表示第i時刻在世界坐標系下的四元數的逆，這里所指的世界坐標系就是前面的C₀坐標系；經過上述第10公式的運算可以得到有關待優化的狀態量的第一項；其中，所述目標函數方程中的第二項為相機的重投影誤差：對上述第9公式中的第二項進行展開換算得到第11公式；

；第11公式 。