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

有效

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

技術(shù)領(lǐng)域

本發(fā)明屬于室內(nèi)組合導(dǎo)航導(dǎo)航系統(tǒng)技術(shù)領(lǐng)域，尤其涉及解決現(xiàn)有的MIMU誤差漂移大導(dǎo)致的導(dǎo)航精度低，可靠性差問題的一種基于CAMERA/MIMU室內(nèi)組合導(dǎo)航系統(tǒng)的四元數(shù)測量方法。

背景技術(shù)

微機(jī)械系統(tǒng)(MEMS)慣性器件存在著測量精度低、誤差隨時間漂移大及溫度穩(wěn)定性差等缺點(diǎn)，MEMS構(gòu)成的微慣性測量單元(MIMU)的導(dǎo)航參數(shù)估計(jì)存在嚴(yán)重誤差漂移，誤差發(fā)散導(dǎo)致系統(tǒng)無法正確進(jìn)行正確導(dǎo)航，最終失去導(dǎo)航的意義。如何抑制誤差漂移，提高導(dǎo)航精度一直是室內(nèi)導(dǎo)航的難題，多傳感器融合是解決此難題的方法，其中，CAMERA輔助的MIMU組合系統(tǒng)是抑制誤差漂移的一種有效方法，采用卡爾曼濾波算法對系統(tǒng)四元數(shù)進(jìn)行估計(jì)，能夠克服MIMU誤差漂移的缺點(diǎn)，提高四元數(shù)精度，進(jìn)而提高導(dǎo)航精度。

發(fā)明內(nèi)容

本發(fā)明實(shí)施例的目的在于提供一種提高導(dǎo)航精度和可靠性的基于CAMERA/MIMU室內(nèi)組合導(dǎo)航系統(tǒng)的四元數(shù)測量方法。

本發(fā)明的目的是這樣實(shí)現(xiàn)的：

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

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

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

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

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

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

步驟七：根據(jù)步驟五中得到的滅點(diǎn)位置矩陣B和步驟六中得到的標(biāo)定矩陣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;]]>