[發明專利]一種基于相機內參在線標定的視覺/慣性組合導航方法

有效

• 申請號: 202010135460.0
• 申請日: 2020-03-02
• 公開（公告）號: CN111156997B
• 公開（公告）日: 2021-11-30
• 發明（設計）人: 楊子寒;賴際舟;呂品;劉建業;袁誠
• 申請（專利權）人: 南京航空航天大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/00
• 代理公司: 南京經緯專利商標代理有限公司 32200
• 代理人: 施昊

1.一種基于相機內參在線標定的視覺/慣性組合導航方法，其特征在于，包括以下步驟：

(1)采集k時刻視覺傳感器數據S(k)、加速度計數據和陀螺儀數據

(2)利用視覺傳感器數據S(k)進行相鄰兩圖像幀之間的特征匹配與檢測；

(3)利用慣性傳感器數據和進行相鄰兩圖像幀之間的預積分：

所述k時刻得到的慣性傳感器數據和包括從k-1時刻到k時刻的加速度計數據和陀螺儀數據其中i＝0,1,2,···,(t(k)-t(k-1))/Δt，t(k)為k時刻對應的采樣時間，t(k-1)為k-1時刻對應的采樣時間，Δt為慣性傳感器的采樣周期，慣性傳感器測量模型：

上式中，n_a和n_ω分別為加速度計和陀螺儀白噪聲；和為加速度計和陀螺儀的隨機游走，其導數為白噪聲；為加速度計量測的理想值，為陀螺儀量測的理想值；g^W為導航系下的重力；為采樣時刻導航坐標系到機體坐標系的旋轉矩陣；

相鄰兩慣性幀之間的預積分過程：

上式中，為位置預積分，為速度預積分，旋轉用四元數γ表示，為旋轉預積分；初始時，和為0，為單位四元數，R(γ)表示將四元數轉化為旋轉矩陣，表示四元數的乘法運算；在相鄰兩圖像幀之間有若干慣性傳感器數據，通過上式，采用迭代的方式對相鄰兩圖像幀之間的所有慣性傳感器數據進行預積分，從而使視覺數據的頻率與慣性預積分頻率保持一致，得到相鄰兩圖像幀之間的慣性預積分和

(4)聯合視覺重投影誤差和慣性預積分誤差優化求解載體導航信息和相機內參，具體過程如下：

(a)建立優化變量X：

X＝[x₀,x₁,···,x_n,x_c,P₀,P₁,···,P_m]

上式中，n為最后一幀的序號，和分別表示第k幀的載體位置、速度、四元數、加速度計和陀螺儀的隨機游走；x_c＝[f_x,f_y,c_x,c_y,k₁,k₂]，f_x和f_y為相機的焦距，c_x和c_y為相機的光心位置，k₁和k₂為相機的徑向畸變參數；P_l,l∈[0,m]為特征點坐標，m為最后一個特征點的序號；

(b)建立優化函數：

其中，||e_p-H_pX||²為邊緣化約束，{e_p,H_p}為邊緣化的先驗信息；為IMU預積分誤差，表示該IMU預積分誤差項是第k-1幀圖像與第k幀圖像之間的IMU預積分誤差，B是所有IMU測量的集合，為IMU預積分誤差的協方差；為相機內參為變量的視覺重投影誤差，表示該視覺重投影誤差項是第k幀圖像上關于第l特征點的視覺重投影誤差，C是所有圖像幀中至少有兩次觀測的特征集，為視覺重投影誤差的協方差；

對于k時刻的第l個特征點P_l，將相機內參建模到視覺重投影誤差中：

上式中，Z_l為在k時刻相機系下Z軸方向的值，K為相機的投影矩陣，表示消除徑向畸變后的特征點位置：

其中，為特征點P_l在像素平面上的實際成像位置；為消除徑向畸變后特征點P_l在像素平面上的位置；x_d、y_d分別為相機圖像坐標系下特征點P_l在x和y方向的值；為k時刻機體系在導航系下的位姿，為相機和慣性傳感器之間的旋轉和平移；

計算視覺重投影誤差關于其相關變量的雅可比矩陣J[0]～J[7]：

上式中，X_l、Y_l、Z_l分別為特征點P_l在k時刻相機系下X、Y、Z軸方向的值；為k時刻機體系在導航系下姿態的四元數表示；K₁的形式如下：

[a]^表示把三維實向量映射為3×3反對稱陣；

相鄰兩圖像幀之間的慣性預積分誤差由預積分預測值與預積分值之差得到：

上式中，為k-1時刻導航坐標系到機體坐標系的旋轉矩陣；分別為k時刻和k-1時刻機體坐標系在導航坐標系下的位置；分別為k時刻和k-1時刻機體坐標系在導航坐標系下的速度；Δt_k為相鄰兩圖像幀之間的時間間隔；分別為k時刻和k-1時刻機體坐標系在導航坐標系下旋轉的四元數；分別為k時刻和k-1時刻機體坐標系下的加速度計隨機游走；分別為k時刻和k-1時刻機體坐標系下的陀螺儀隨機游走，[γ]_xyz表示取四元數γ的x、y、z分量；

(c)使用高斯-牛頓法對優化函數進行迭代求解，當達到誤差收斂或者設定的最大迭代次數時停止迭代，求得載體導航信息和相機內參；

(5)輸出載體導航信息和相機內參，并返回步驟(1)。