視覺輔助慣性測量單元的數字頭盔顯示設備跟蹤系統的製作方法
2023-05-31 08:45:26 1
視覺輔助慣性測量單元的數字頭盔顯示設備跟蹤系統的製作方法
【專利摘要】本發明公開了一種基於視覺輔助慣性測量單元的數字頭盔跟蹤系統,結合慣性測量單元和視覺測量各自的優點,自主、實時地實現頭盔姿態的跟蹤。本發明的新穎之處在於利用視覺輔助的方法,修正慣性測量隨時間而不斷積累的誤差,實現對數字頭盔顯示器姿態和位置的實時跟蹤計算。本發明的方法考慮了測量過程中的噪聲影響,利用卡爾曼濾波算法進行頭盔姿態的估計,儘可能地減小噪聲對測量結果的影響。本發明具有自主性好,實時性好,計算量小,成本低,質量輕,負荷小,使用方便,精度較高等優點。
【專利說明】視覺輔助慣性測量單元的數字頭盔顯示設備跟蹤系統
【技術領域】
[0001] 本發明與計算機視覺和慣性測量相關,屬於視覺測量和組合導航研究領域,建立 的頭盔顯示器跟蹤系統具有輕質、舒適、快響應、高精度等特點,其中採用的是一種視覺輔 助慣性測量單元的導航方法,能夠實時、可靠地實現對頭盔姿態的精確跟蹤。
【背景技術】
[0002] 頭盔跟蹤系統通過跟蹤飛行員頭部運動,實現武器系統的瞄準,在實際飛行任務 中,飛行員不僅要時刻查看儀器面板數據變化,同時還要關注艙外打擊目標的位置,視線瞄 準目標,因此通過飛行員視線方向變化控制瞄準系統瞄準目標的方法是非常有意義的,飛 行員視線方向體現在飛行員所帶頭盔的姿態變化,利用傳感器獲取頭部姿態,從而得到視 線方向,轉化成控制信號,控制瞄準系統。
[0003] 頭盔跟蹤系統應用在頭盔瞄準具中已有很長一段時間,傳統的瞄準方式主要有機 電式、電磁式和光電式:機電式是通過連杆裝置和電磁離合器將頭盔與座艙頂部連接起來, 由連杆兩端的測角器測量頭部轉動的角度;電磁式是通過頭盔和座艙內的磁敏感器和磁輻 射器,利用磁場方向的偏差獲得頭部相對於機身正方向的角度,電磁設備的安裝會使得飛 行員頭部負荷大,且易對艙內其他設備產生電磁幹擾;光電式是通過座艙內的紅外接收機 接收頭盔兩側紅外二極體發出的紅外信號,與基準信號比對,由偏差計算頭部轉動的角度, 雖然這種方法較為先進,也改進了前兩種方法的一些缺點,但是光電式的瞄準系統結構復 雜,安裝調試難度大,也為頭盔瞄準器的使用造成一定的不便。
[0004] 隨著計算機視覺的發展,越來越多的學者將目光轉向了視覺測量,其用於頭盔瞄 準具的應用價值也逐漸體現,視覺測量由於其成本低、質量輕、可靠性高、使用方便、技術成 熟等優點,大大減輕飛行員的負擔和安裝調試的難度,因此,許多學者對基於視覺測量、圖 像處理技術的頭盔瞄準具進行了大量的仿真與研究,但是視覺圖像的處理需要佔用很大的 計算空間,處理速度較慢,若要滿足視線對準的快速性與實時性,必須要求機載計算機具有 很大的存儲空間和很高的計算效率。
[0005] 慣性測量單元基於慣性測量技術,是非常成熟的測量手段,已廣泛應用於各類飛 行器和艦船,它具有不依賴外界信息、不向外界輻射能量、不受幹擾、隱蔽性好、能連續不斷 地提供數據等特點,是一種完全自主性、實時性的測量、導航方法,但慣性測量單元的測量 誤差隨時間的不斷增加而積累,再加上自身的漂移特性,不能滿足長時間的精度要求,本發 明的方法就是基於視覺輔助慣性測量單元的方法,利用成熟的慣性測量單元測量技術,力口 以視覺測量的輔助,以一定的時間間隔修正慣性測量的誤差,以達到精確跟蹤頭盔姿態的 目的。
【發明內容】
[0006] 利用慣性測量單元而發展起來的各項技術憑藉其自主性、實時性等強大優點,保 證了慣性測量在數字頭盔姿態跟蹤方面的可行性;視覺測量技術的發展也為數字頭盔的姿 態跟蹤提供了技術支持,其精度較高的優勢在保證姿態跟蹤自主性的基礎上,彌補慣性測 量單元誤差隨時間積累的缺點,輔助慣性測量單元實現對數字頭盔姿態的實時跟蹤;本發 明設計一種視覺輔助慣性測量單元的數字頭盔跟蹤系統,在頭盔頂部選定若干個標誌/特 徵點進行標定,相機安裝在頭盔頂部正上方的座艙內,對正下方進行拍照;慣性測量單元安 裝在飛行員的頭盔上,通過建立一種導航濾波方法實現對頭盔姿態的計算。
[0007] 本發明的基本原理如下。
[0008] 一、坐標系統的建立 為方便參數的表示和轉換,建立慣性坐標系、相機坐標系、像素坐標系、頭盔初始坐標 系、頭部轉動坐標系等五個坐標系;慣性測量單元的測量值在慣性坐標系下表示;相機測 量到的像點坐標在像素坐標系下表示;頭盔初始坐標系以頭盔中心為坐標原點,以平視前 方的視線方向為X軸,以向上指向相機的方向為Z軸,y軸與X軸、z軸構成右手坐標系,且 不隨頭部轉動而轉動;頭部轉動坐標系隨頭部一起轉動,以頭盔最底部中心(頸部)為坐標 原點,平視前方時的三軸方向與頭盔初始坐標系三軸方向一致。這五個坐標系之間,存在一 定的轉換關係。
[0009] 二、慣性測量單元模型 慣性測量單元(陀螺儀和加速度計)的測量值為頭盔在慣性空間內的轉動角速度和非 引力加速度\沿頭盔三個轉軸方向的分量,而《和a則表示頭盔在慣性空間內真實的、不 含噪聲的轉動角速度和加速度,由於慣性測量單元存在漂移,且在測量的過程中受到隨機 噪聲的幹擾,因此實際測量值和 aj?與真實值ω和a有如下關係,
【權利要求】
1. 本發明權利要求一種視覺輔助慣性測量單元的數字頭盔跟蹤系統,這套系統的特徵 包括:數字頭盔跟蹤系統在頭盔頂部選定若干個標誌/特徵點進行標定,視覺相機安裝在 頭盔頂部正上方的座艙內,對正下方的數字頭盔進行拍照,慣性測量單元安裝在飛行員的 頭盔上,慣性測量單元測量頭盔在運動中的角速度信息,視覺相機測量頭盔頂部已知特徵 點的坐標,慣性測量單元不間斷地對頭盔姿態進行測量,視覺相機每隔一固定的時間間隔 對視野內出現的頭盔頂部特徵點進行拍照,經過特徵提取、匹配後,獲得特徵點坐標值,對 此時的慣性測量誤差予以修正。
2. 本發明權利要求一種慣性測量單元和視覺測量的模型建立方法 慣性測量單元(陀螺儀和加速度計)的測量值為頭盔在慣性空間內的轉動角速度 和非引力加速度\沿頭盔三個轉軸方向的分量,而m和》則表示頭盔在慣性空間內真 實的、不含噪聲的轉動角速度和加速度,由於慣性測量單元存在漂移,且在測量的過程中受 到隨機噪聲的幹擾,因此實際測量值和&為
其中,為頭部轉動坐標系$相對於頭盔初始坐標系G的姿態四元數對應的旋轉矩 陣,g"5為重力加速度,\和na分別為陀螺儀和加速度計的零均值測量高斯白噪聲,1^和 分別為陀螺儀和加速度計的漂移,頭盔頂部的特徵點在頭部轉動坐標系S中的三維坐 標Pf是固定且已知的,由此可以得到相機到特徵點的位置矢量Pf在相機坐標系下的分量 表完
其中,P?為相機在頭盔初始坐標系G下的位置矢量,Pi為頭部轉動坐標系S在頭盔 初始坐標系G下的位置矢量,為相機坐標系e相對於頭部轉動坐標系j的旋轉四元數 對應的旋轉矩陣,利用相機的成像透射原理以及特徵點坐標在像素坐標系下的投影關係, 可得到測量坐標矢量z
z為頭盔頂部特徵點4投影到像素坐標系中的二維坐標,為特徵檢測過程中產生的 誤差。
3. 本發明權利要求一種頭盔姿態運動表示方法 利用姿態四元數來描述頭盔的姿態,並滿足歸一化的約束條件,則姿態運動學方程表 示為
始坐標系G的轉動角速度(在S系下的分量),可由慣性測量單元的測量值求得;=[分〇 ft分2分3]為頭部轉動坐標系S相對於頭盜初始坐標系G的姿態四兀數,分〇為 的標量部分,+ 夬為?js的矢量部分,F為5的反對稱陣,利用四級四階龍格 庫塔法求解式(4)或(5)姿態運動學方程,即可得到頭部轉動坐標系S相對頭盔初始坐標 系G的姿態四元數,頭部轉動坐標系S相對頭盔初始坐標系G的位置和速度均不變,將系 統的微分方程組整理為
4.本發明權利要求一種卡爾曼濾波算法以實現對姿態的計算 估計過程中利用了如下信息:系統方程、觀測方程、白噪聲激勵的統計特性、量測誤差 的統計特性,本發明建立的離散型卡爾曼濾波算法如下: 設%+1時刻的估計狀態受系統噪聲%驅動,狀態方程 描述了噪聲的驅動機理,
的量測與4+r滿足線性關係,即觀測方程
其中,為%時刻的估計狀態,為%時刻至時刻的一步轉移陣,為系統噪 聲驅動陣,%為系統噪聲,為量測陣,G為量測噪聲
其中,為系統噪聲方差陣,A為量測噪聲方差陣 如果被估計狀態1^!滿足式(7),量測量4+1滿足式(8),系統噪聲%和量測噪聲^滿 足式(9),系統噪聲方差陣a非負定,量測噪聲方差陣為.正定,先+1時刻的量測為&+1,則 義+1的估計ffc+1按下述方程求解:
式(10)即為離散型卡爾曼濾波基本方程,只要給定初值i〇和馬,根據fc + l時刻的量 測4+1,就可遞推計算得到i + 1時刻的狀態估計。
5.本發明權利要求一種擴展卡爾曼濾波算法以實現對姿態的計算 權利要求4給出的卡爾曼濾波只能用於線性隨機系統的狀態估計,權利要求5要求的 擴展卡爾曼濾波是對非線性系統方程進行線性化近似後,再利用普通卡爾曼濾波方法進行 濾波估計的算法,在使用離散型卡爾曼濾波基本方程之前,對系統方程和量測方程做離散 化和線性化處理,考慮一般化的連續非線性系統,線性化的狀態方程可寫為
| 聲 設定系統的狀態向量為x,其中包含頭部轉動坐標系S相對頭盔初始坐標系G的姿態 四元數的矢量部分ft 和陀螺儀的漂移狀態向量為
由式(5)和(6)可將狀態方程可表示為
所以,狀態轉移矩陣可由式(14)對狀態向量x求雅可比矩陣得到,
其中,Qs(<)為陀螺儀的零均值測量高斯白噪聲的方差陣,為陀螺儀的零均值驅 動高斯白噪聲的方差陣;系統噪聲輸入矩陣為
將系統離散化後得到狀態轉移矩陣,
考慮到濾波周期T較短,可看作常陣,且根據擴展卡爾曼濾波的線性化方法,舍 去二階及以上項,可得到狀態轉移矩陣為
其中,7 = ^4-^,系統噪聲輸入陣為
系統噪聲方差陣為
其中,=5[w(l)w(if],當濾波周期^較短時,G〇G⑷= Gfc 由此 系統噪聲方差陣可簡化為
量測陣H可由量測方程對狀態向量求雅可比矩陣得到,
式中
當有*個已知位置特徵點時,量測陣H可以表示成以下形式,
在估計過程中,利用t時刻的估計值,並根據t+1時刻的量測量Zw,即可遞推得 至ij先+1時刻的狀態估計i^+1。
【文檔編號】A42B3/04GK104280022SQ201310295855
【公開日】2015年1月14日 申請日期:2013年7月13日 優先權日:2013年7月13日
【發明者】張澤旭, 劉蕾, 鄭博 申請人:哈爾濱點石仿真科技有限公司