視覺輔助慣性測量單元的數(shù)字頭盔顯示設(shè)備跟蹤系統(tǒng)的制作方法
【專利摘要】本發(fā)明公開了一種基于視覺輔助慣性測量單元的數(shù)字頭盔跟蹤系統(tǒng)，結(jié)合慣性測量單元和視覺測量各自的優(yōu)點，自主、實時地實現(xiàn)頭盔姿態(tài)的跟蹤。本發(fā)明的新穎之處在于利用視覺輔助的方法，修正慣性測量隨時間而不斷積累的誤差，實現(xiàn)對數(shù)字頭盔顯示器姿態(tài)和位置的實時跟蹤計算。本發(fā)明的方法考慮了測量過程中的噪聲影響，利用卡爾曼濾波算法進行頭盔姿態(tài)的估計，盡可能地減小噪聲對測量結(jié)果的影響。本發(fā)明具有自主性好，實時性好，計算量小，成本低，質(zhì)量輕，負荷小，使用方便，精度較高等優(yōu)點。
【專利說明】視覺輔助慣性測量單元的數(shù)字頭盔顯示設(shè)備跟蹤系統(tǒng)

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明與計算機視覺和慣性測量相關(guān)，屬于視覺測量和組合導(dǎo)航研究領(lǐng)域，建立 的頭盔顯示器跟蹤系統(tǒng)具有輕質(zhì)、舒適、快響應(yīng)、高精度等特點，其中采用的是一種視覺輔 助慣性測量單元的導(dǎo)航方法，能夠?qū)崟r、可靠地實現(xiàn)對頭盔姿態(tài)的精確跟蹤。

【背景技術(shù)】
[0002] 頭盔跟蹤系統(tǒng)通過跟蹤飛行員頭部運動，實現(xiàn)武器系統(tǒng)的瞄準，在實際飛行任務(wù) 中，飛行員不僅要時刻查看儀器面板數(shù)據(jù)變化，同時還要關(guān)注艙外打擊目標的位置，視線瞄 準目標，因此通過飛行員視線方向變化控制瞄準系統(tǒng)瞄準目標的方法是非常有意義的，飛 行員視線方向體現(xiàn)在飛行員所帶頭盔的姿態(tài)變化，利用傳感器獲取頭部姿態(tài)，從而得到視 線方向，轉(zhuǎn)化成控制信號，控制瞄準系統(tǒng)。
[0003] 頭盔跟蹤系統(tǒng)應(yīng)用在頭盔瞄準具中已有很長一段時間，傳統(tǒng)的瞄準方式主要有機 電式、電磁式和光電式：機電式是通過連桿裝置和電磁離合器將頭盔與座艙頂部連接起來， 由連桿兩端的測角器測量頭部轉(zhuǎn)動的角度；電磁式是通過頭盔和座艙內(nèi)的磁敏感器和磁輻 射器，利用磁場方向的偏差獲得頭部相對于機身正方向的角度，電磁設(shè)備的安裝會使得飛 行員頭部負荷大，且易對艙內(nèi)其他設(shè)備產(chǎn)生電磁干擾；光電式是通過座艙內(nèi)的紅外接收機 接收頭盔兩側(cè)紅外二極管發(fā)出的紅外信號，與基準信號比對，由偏差計算頭部轉(zhuǎn)動的角度， 雖然這種方法較為先進，也改進了前兩種方法的一些缺點，但是光電式的瞄準系統(tǒng)結(jié)構(gòu)復(fù) 雜，安裝調(diào)試難度大，也為頭盔瞄準器的使用造成一定的不便。
[0004] 隨著計算機視覺的發(fā)展，越來越多的學(xué)者將目光轉(zhuǎn)向了視覺測量，其用于頭盔瞄 準具的應(yīng)用價值也逐漸體現(xiàn)，視覺測量由于其成本低、質(zhì)量輕、可靠性高、使用方便、技術(shù)成 熟等優(yōu)點，大大減輕飛行員的負擔和安裝調(diào)試的難度，因此，許多學(xué)者對基于視覺測量、圖 像處理技術(shù)的頭盔瞄準具進行了大量的仿真與研究，但是視覺圖像的處理需要占用很大的 計算空間，處理速度較慢，若要滿足視線對準的快速性與實時性，必須要求機載計算機具有 很大的存儲空間和很高的計算效率。
[0005] 慣性測量單元基于慣性測量技術(shù)，是非常成熟的測量手段，已廣泛應(yīng)用于各類飛 行器和艦船，它具有不依賴外界信息、不向外界輻射能量、不受干擾、隱蔽性好、能連續(xù)不斷 地提供數(shù)據(jù)等特點，是一種完全自主性、實時性的測量、導(dǎo)航方法，但慣性測量單元的測量 誤差隨時間的不斷增加而積累，再加上自身的漂移特性，不能滿足長時間的精度要求，本發(fā) 明的方法就是基于視覺輔助慣性測量單元的方法，利用成熟的慣性測量單元測量技術(shù)，力口 以視覺測量的輔助，以一定的時間間隔修正慣性測量的誤差，以達到精確跟蹤頭盔姿態(tài)的 目的。


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

[0006] 利用慣性測量單元而發(fā)展起來的各項技術(shù)憑借其自主性、實時性等強大優(yōu)點，保 證了慣性測量在數(shù)字頭盔姿態(tài)跟蹤方面的可行性；視覺測量技術(shù)的發(fā)展也為數(shù)字頭盔的姿 態(tài)跟蹤提供了技術(shù)支持，其精度較高的優(yōu)勢在保證姿態(tài)跟蹤自主性的基礎(chǔ)上，彌補慣性測 量單元誤差隨時間積累的缺點，輔助慣性測量單元實現(xiàn)對數(shù)字頭盔姿態(tài)的實時跟蹤；本發(fā) 明設(shè)計一種視覺輔助慣性測量單元的數(shù)字頭盔跟蹤系統(tǒng)，在頭盔頂部選定若干個標志/特 征點進行標定，相機安裝在頭盔頂部正上方的座艙內(nèi)，對正下方進行拍照；慣性測量單元安 裝在飛行員的頭盔上，通過建立一種導(dǎo)航濾波方法實現(xiàn)對頭盔姿態(tài)的計算。
[0007] 本發(fā)明的基本原理如下。
[0008] 一、坐標系統(tǒng)的建立 為方便參數(shù)的表示和轉(zhuǎn)換，建立慣性坐標系、相機坐標系、像素坐標系、頭盔初始坐標 系、頭部轉(zhuǎn)動坐標系等五個坐標系；慣性測量單元的測量值在慣性坐標系下表示；相機測 量到的像點坐標在像素坐標系下表示；頭盔初始坐標系以頭盔中心為坐標原點，以平視前 方的視線方向為X軸，以向上指向相機的方向為Z軸，y軸與X軸、z軸構(gòu)成右手坐標系，且 不隨頭部轉(zhuǎn)動而轉(zhuǎn)動；頭部轉(zhuǎn)動坐標系隨頭部一起轉(zhuǎn)動，以頭盔最底部中心(頸部）為坐標 原點，平視前方時的三軸方向與頭盔初始坐標系三軸方向一致。這五個坐標系之間，存在一 定的轉(zhuǎn)換關(guān)系。
[0009] 二、慣性測量單元模型 慣性測量單元(陀螺儀和加速度計）的測量值為頭盔在慣性空間內(nèi)的轉(zhuǎn)動角速度和非 引力加速度\沿頭盔三個轉(zhuǎn)軸方向的分量，而《和a則表示頭盔在慣性空間內(nèi)真實的、不 含噪聲的轉(zhuǎn)動角速度和加速度，由于慣性測量單元存在漂移，且在測量的過程中受到隨機 噪聲的干擾，因此實際測量值和 aj?與真實值ω和a有如下關(guān)系，

【權(quán)利要求】
1. 本發(fā)明權(quán)利要求一種視覺輔助慣性測量單元的數(shù)字頭盔跟蹤系統(tǒng)，這套系統(tǒng)的特征 包括：數(shù)字頭盔跟蹤系統(tǒng)在頭盔頂部選定若干個標志/特征點進行標定，視覺相機安裝在 頭盔頂部正上方的座艙內(nèi)，對正下方的數(shù)字頭盔進行拍照，慣性測量單元安裝在飛行員的 頭盔上，慣性測量單元測量頭盔在運動中的角速度信息，視覺相機測量頭盔頂部已知特征 點的坐標，慣性測量單元不間斷地對頭盔姿態(tài)進行測量，視覺相機每隔一固定的時間間隔 對視野內(nèi)出現(xiàn)的頭盔頂部特征點進行拍照，經(jīng)過特征提取、匹配后，獲得特征點坐標值，對 此時的慣性測量誤差予以修正。
2. 本發(fā)明權(quán)利要求一種慣性測量單元和視覺測量的模型建立方法 慣性測量單元(陀螺儀和加速度計）的測量值為頭盔在慣性空間內(nèi)的轉(zhuǎn)動角速度 和非引力加速度\沿頭盔三個轉(zhuǎn)軸方向的分量，而m和》則表示頭盔在慣性空間內(nèi)真 實的、不含噪聲的轉(zhuǎn)動角速度和加速度，由于慣性測量單元存在漂移，且在測量的過程中受 到隨機噪聲的干擾，因此實際測量值和&為
其中，為頭部轉(zhuǎn)動坐標系$相對于頭盔初始坐標系G的姿態(tài)四元數(shù)對應(yīng)的旋轉(zhuǎn)矩 陣，g"5為重力加速度，\和na分別為陀螺儀和加速度計的零均值測量高斯白噪聲，1^和 分別為陀螺儀和加速度計的漂移，頭盔頂部的特征點在頭部轉(zhuǎn)動坐標系S中的三維坐 標Pf是固定且已知的，由此可以得到相機到特征點的位置矢量Pf在相機坐標系下的分量 表完
其中，P?為相機在頭盔初始坐標系G下的位置矢量，Pi為頭部轉(zhuǎn)動坐標系S在頭盔 初始坐標系G下的位置矢量，為相機坐標系e相對于頭部轉(zhuǎn)動坐標系j的旋轉(zhuǎn)四元數(shù) 對應(yīng)的旋轉(zhuǎn)矩陣，利用相機的成像透射原理以及特征點坐標在像素坐標系下的投影關(guān)系， 可得到測量坐標矢量z
z為頭盔頂部特征點4投影到像素坐標系中的二維坐標，為特征檢測過程中產(chǎn)生的 誤差。
3. 本發(fā)明權(quán)利要求一種頭盔姿態(tài)運動表示方法 利用姿態(tài)四元數(shù)來描述頭盔的姿態(tài)，并滿足歸一化的約束條件，則姿態(tài)運動學(xué)方程表 示為
始坐標系G的轉(zhuǎn)動角速度(在S系下的分量)，可由慣性測量單元的測量值求得；=[分〇 ft分2分3]為頭部轉(zhuǎn)動坐標系S相對于頭盜初始坐標系G的姿態(tài)四兀數(shù)，分〇為 的標量部分，+ 夬為？js的矢量部分，F(xiàn)為5的反對稱陣，利用四級四階龍格 庫塔法求解式（4)或（5)姿態(tài)運動學(xué)方程，即可得到頭部轉(zhuǎn)動坐標系S相對頭盔初始坐標 系G的姿態(tài)四元數(shù)，頭部轉(zhuǎn)動坐標系S相對頭盔初始坐標系G的位置和速度均不變，將系 統(tǒng)的微分方程組整理為
4.本發(fā)明權(quán)利要求一種卡爾曼濾波算法以實現(xiàn)對姿態(tài)的計算 估計過程中利用了如下信息：系統(tǒng)方程、觀測方程、白噪聲激勵的統(tǒng)計特性、量測誤差 的統(tǒng)計特性，本發(fā)明建立的離散型卡爾曼濾波算法如下： 設(shè)％+1時刻的估計狀態(tài)受系統(tǒng)噪聲％驅(qū)動，狀態(tài)方程 描述了噪聲的驅(qū)動機理，
的量測與4+r滿足線性關(guān)系，即觀測方程
其中，為％時刻的估計狀態(tài)，為％時刻至?xí)r刻的一步轉(zhuǎn)移陣，為系統(tǒng)噪 聲驅(qū)動陣，％為系統(tǒng)噪聲，為量測陣，G為量測噪聲
其中，為系統(tǒng)噪聲方差陣，A為量測噪聲方差陣 如果被估計狀態(tài)1^!滿足式（7)，量測量4+1滿足式（8)，系統(tǒng)噪聲％和量測噪聲^滿 足式（9)，系統(tǒng)噪聲方差陣a非負定，量測噪聲方差陣為.正定，先+1時刻的量測為&+1，則 義+1的估計ffc+1按下述方程求解：
式（10)即為離散型卡爾曼濾波基本方程，只要給定初值i〇和馬，根據(jù)fc + l時刻的量 測4+1，就可遞推計算得到i + 1時刻的狀態(tài)估計。
5.本發(fā)明權(quán)利要求一種擴展卡爾曼濾波算法以實現(xiàn)對姿態(tài)的計算 權(quán)利要求4給出的卡爾曼濾波只能用于線性隨機系統(tǒng)的狀態(tài)估計，權(quán)利要求5要求的 擴展卡爾曼濾波是對非線性系統(tǒng)方程進行線性化近似后，再利用普通卡爾曼濾波方法進行 濾波估計的算法，在使用離散型卡爾曼濾波基本方程之前，對系統(tǒng)方程和量測方程做離散 化和線性化處理，考慮一般化的連續(xù)非線性系統(tǒng)，線性化的狀態(tài)方程可寫為
| 聲 設(shè)定系統(tǒng)的狀態(tài)向量為x，其中包含頭部轉(zhuǎn)動坐標系S相對頭盔初始坐標系G的姿態(tài) 四元數(shù)的矢量部分ft 和陀螺儀的漂移狀態(tài)向量為
由式（5)和（6)可將狀態(tài)方程可表示為
所以，狀態(tài)轉(zhuǎn)移矩陣可由式（14)對狀態(tài)向量x求雅可比矩陣得到，
其中，Qs(<)為陀螺儀的零均值測量高斯白噪聲的方差陣，為陀螺儀的零均值驅(qū) 動高斯白噪聲的方差陣；系統(tǒng)噪聲輸入矩陣為
將系統(tǒng)離散化后得到狀態(tài)轉(zhuǎn)移矩陣，
考慮到濾波周期T較短，可看作常陣，且根據(jù)擴展卡爾曼濾波的線性化方法，舍 去二階及以上項，可得到狀態(tài)轉(zhuǎn)移矩陣為
其中，7 = ^4-^，系統(tǒng)噪聲輸入陣為
系統(tǒng)噪聲方差陣為
其中，=5[w(l)w(if],當濾波周期^較短時，G〇G⑷= Gfc 由此 系統(tǒng)噪聲方差陣可簡化為
量測陣H可由量測方程對狀態(tài)向量求雅可比矩陣得到，
式中
當有*個已知位置特征點時，量測陣H可以表示成以下形式，
在估計過程中，利用t時刻的估計值,并根據(jù)t+1時刻的量測量Zw,即可遞推得 至ij先+1時刻的狀態(tài)估計i^+1。
【文檔編號】A42B3/04GK104280022SQ201310295855
【公開日】2015年1月14日 申請日期:2013年7月13日 優(yōu)先權(quán)日:2013年7月13日
【發(fā)明者】張澤旭, 劉蕾, 鄭博 申請人:哈爾濱點石仿真科技有限公司