[發(fā)明專利]一種基于CAMERA/MIMU室內(nèi)組合導(dǎo)航系統(tǒng)的四元數(shù)測量方法

有效

• 申請?zhí)枺?/td>: 201410172298.4
• 申請日: 2014-04-25
• 公開（公告）號: CN103925926A
• 公開（公告）日: 2014-07-16
• 發(fā)明（設(shè)計）人: 于飛;白紅美;于春陽;周廣濤;阮雙雙;李佳璇;張麗麗
• 申請（專利權(quán)）人: 哈爾濱工程大學(xué)
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于CAMERA/MIMU室內(nèi)組合導(dǎo)航系統(tǒng)的四元數(shù)測量方法，其特征在于：

步驟一：連接CAMERA/MIMU室內(nèi)組合導(dǎo)航系統(tǒng)，系統(tǒng)上電后進行預(yù)熱；

步驟二：采集陀螺輸出的角速度和CAMERA拍攝的圖像；

步驟三：采用空間濾波方法生成3×3模板的高斯卷積濾波器對步驟二中CAMERA拍攝的圖像進行平滑濾波；

步驟四：采用邊緣檢測器追蹤步驟三中經(jīng)平滑濾波后圖像中亮度快速變化的點，得到原始亮度數(shù)據(jù)的梯度值，并利用局部梯度值的最大值定位物體的邊緣；

步驟五：采用隨機采樣一致性方法找到步驟三中經(jīng)平滑濾波后圖像中的滅點位置，確定中心滅點、水平滅點和垂直滅點位置矩陣B；

步驟六：根據(jù)CAMERA焦距、像主點和透鏡畸變參數(shù)測量出標定矩陣K；

步驟七：根據(jù)步驟五中得到的滅點位置矩陣B和步驟六中得到的標定矩陣K，得到CAMERA數(shù)學(xué)模型：

B＝KA；

式中，A為CAMERA旋轉(zhuǎn)矩陣：

A=q02+q12-q22-q322(q1q2-q0q3)2(q1q3+q0q2)2(q1q2+q0q3)q02-q12+q22-q322(q2q3-q0q1)2(q1q3-q0q2)2(q2q3+q0q1)q02-q12-q22+q32;]]>

其中，為四元數(shù)，q₀，q₁，q₂，q₃分別為四個實數(shù)，為載體坐標系三個正交坐標軸上的單位矢量；

步驟八：利用步驟七中獲得的CAMERA數(shù)學(xué)模型確定CAMERA/MIMU室內(nèi)組合導(dǎo)航系統(tǒng)的量測方程：

B_k＝KA_k+v_k；

式中，B_k為t_k時刻滅點位置矩陣、A_k為t_k時刻與四元數(shù)相關(guān)的CAMERA旋轉(zhuǎn)矩陣，v_k為量測噪聲序列；

步驟九：根據(jù)步驟二中陀螺輸出的角速度測量矩陣W_k-1：

Wk-1=I+[Φa]+12[Φa]2;]]>

式中，I為單位陣，[Φ_a]為由旋轉(zhuǎn)矢量構(gòu)造的反對稱矩陣，Δt＝t_k-t_k-1為更新周期；

步驟十：根據(jù)步驟八中得到的矩陣W_k-1得到MIMU的離散域四元數(shù)Q_k更新模型：

Qk=12QWk-1Qk-1;]]>

式中，φ_x，φ_y，φ_z為旋轉(zhuǎn)矢量Φ在載體坐標系三個正交坐標軸上的分量；

步驟十一：選取四元數(shù)Q_k＝[q_0,k?q_1,k?q_2,k?q_3,k]^T作為CAMERA/MIMU室內(nèi)組合導(dǎo)航系統(tǒng)的狀態(tài)向量，利用步驟十中獲得的四元數(shù)更新模型確定CAMERA/MIMU室內(nèi)組合導(dǎo)航系統(tǒng)的狀態(tài)方程為：

x_k＝F_k-1x_k-1+w_k-1；

式中，q_i,k(i＝0,1,2,3)為t_k時刻四元數(shù)的分量值，w_k-1為系統(tǒng)激勵噪聲序列，系統(tǒng)狀態(tài)向量x_k＝Q_k，一步轉(zhuǎn)移矩陣

步驟十二：根據(jù)步驟八和步驟十一建立的CAMERA/MIMU室內(nèi)組合導(dǎo)航系統(tǒng)的狀態(tài)方程和量測方程進行卡爾曼濾波，估計出四元數(shù)Q_k。