[發明專利]一種視覺慣性里程計中的加權歐拉預積分方法

有效

• 申請號: 201810239808.3
• 申請日: 2018-03-22
• 公開（公告）號: CN108731700B
• 公開（公告）日: 2020-07-31
• 發明（設計）人: 潘樹國;曾攀;王帥;黃礪梟
• 申請（專利權）人: 東南大學
• 主分類號: G01C22/00
• 分類號: G01C22/00;G06T7/73
• 代理公司: 南京蘇高專利商標事務所(普通合伙) 32204
• 代理人: 孟紅梅

1.一種視覺慣性里程計中的加權歐拉預積分方法，其特征在于：該方法首先在積分運動模型上使用加權歐拉預積分，然后將兩關鍵幀間的IMU測量值通過迭代求和處理，得到兩關鍵幀之間的運動約束，再通過公式處理進一步得到相對運動增量模型以及預積分測量模型；

將積分運動模型迭代公式使用加權歐拉預積分處理后表示為：

其中Δt是兩次IMU測量值之間的時間間隔；R，v，p分別代表旋轉、速度和平移，ω，a分別代表角速度和加速度；R_wB表示從Body坐標系到世界坐標系的旋轉；下標W和B分別表示世界坐標系和Body坐標系，()^表示將向量轉化為反對稱矩陣；上標T表示轉置；

在數值積分中，兩個IMU測量時刻間的角速度和加速度計算公式為：

ω_B(t，t+Δt)＝c₁(t)ω_B(t)+c₂(t)ω_B(t+Δt)

a_B(t，t+Δt)＝c₃(t)a_B(t)+c₄(t)a_B(t+Δt)

其中，c₁-c₄為權值系數，系數的計算：

c₁(t)＝ω_B(t)/(ω_B(t)+ω_B(t+Δt))

c₂(t)＝ω_B(t+Δt)/(ω_B(t)+ω_B(t+Δt))

c₃(t)＝a_B(t)/(a_B(t)+a_B(t+Δt))

c₄(t)＝a_B(t+Δt)/(a_B(t)+a_B(t+Δt))

通過對兩個連續視覺關鍵幀間的IMU測量值迭代求和，得到兩個相鄰關鍵幀i，j之間的約束為：

其中，R,v,p分別代表旋轉、速度和平移；g為重力加速度；為加權后的IMU測量的角速度和加速度，b^g、b^a為對應的偏置，η^g,η^a為對應的噪聲。

2.根據權利要求1所述的一種視覺慣性里程計中的加權歐拉預積分方法，其特征在于：所述相對運動增量模型表示為：

其中，上標T表示轉置；ΔR_ij、Δv_ij、Δp_ij分別表示關鍵幀i,j之間相對旋轉、速度和平移。

3.根據權利要求2所述的一種視覺慣性里程計中的加權歐拉預積分方法，其特征在于：所述預積分測量模型表示為：

其中，分別表示含有噪聲項的關鍵幀i,j之間相對旋轉、速度和平移，δv_ij，δp_ij分別表示相應的噪聲項，表示j-1幀的右雅可比。

4.根據權利要求3所述的一種視覺慣性里程計中的加權歐拉預積分方法，其特征在于：

還包括根據如下公式獲得協方差矩陣Σ_ij：

其中，和代表高斯噪聲的協方差矩陣，具體形式為：

5.根據權利要求4所述的一種視覺慣性里程計中的加權歐拉預積分方法，其特征在于：

還包括根據如下公式獲得雅可比矩陣J_j：

其中，

分別表示偏置發生改變之前關鍵幀i,j之間的相對旋轉、速度和平移的預積分值。