本發(fā)明涉及虛擬現(xiàn)實(virtualreality，vr)技術，尤其涉及一種基于虛擬現(xiàn)實(vr)技術的空間定位定姿系統(tǒng)及其方法。

背景技術：

虛擬現(xiàn)實(vr)技術，是利用電腦或其他智能計算設備模擬產(chǎn)生一個三維空間的虛擬世界，提供關于視覺、聽覺、觸覺等人體感官的模擬，讓用戶如同身臨其境一般，即“完全在場感”。要實現(xiàn)這種“完全在場感”的使用體驗，除了對vr系統(tǒng)的視覺性能指標有所要求外，還要求vr系統(tǒng)可以提供空間定位定姿功能。定位即確定載體，如vr頭盔、vr控制器、道具等在空間中包含三個自由度的位置信息；定姿即確定載體在空間中包含三個自由度的方向信息，通常使用歐拉角、四元數(shù)或旋轉矩陣來表達載體的姿態(tài)信息。

虛擬現(xiàn)實系統(tǒng)中基于慣性傳感器的空間定位定姿系統(tǒng)，通常包括9軸慣性測量單元(imu)慣性傳感器模組、無線傳輸模塊、主控及圖形計算單元、頭戴式顯示頭盔?？臻g定位的一般工作過程為，首先對imu慣性傳感器測量得到的加速度數(shù)據(jù)進行積分，故而得到載體速度數(shù)據(jù)，再對速度數(shù)據(jù)進行積分得到載體位移數(shù)據(jù)，進而獲得載體在空間中的相對位置信息。

在現(xiàn)有技術條件下，實現(xiàn)基于慣性傳感器針對虛擬現(xiàn)實系統(tǒng)的空間定位定姿技術時，由于慣性傳感器固有的誤差會在積分過程中累積，繼而會導致位置漂移，最終導致系統(tǒng)無法連續(xù)提供準確的空間位置數(shù)據(jù)和達到實現(xiàn)虛擬現(xiàn)實系統(tǒng)的高精度、實時性要求，因此無法實現(xiàn)虛擬現(xiàn)實系統(tǒng)能提供“完全在場”的使用體驗。

技術實現(xiàn)要素：

有鑒于此，本發(fā)明的主要目的在于提供一種基于虛擬現(xiàn)實(vr)技術的空間定位定姿系統(tǒng)及其方法，以克服現(xiàn)有基于慣性傳感器的vr系統(tǒng)空間定位定姿系統(tǒng)中存在固有測量誤差并隨積分過程向下積累，從而導致位置漂移以至無法提供連續(xù)準確的空間位置測量數(shù)據(jù)的技術缺陷。

本發(fā)明的另一目的在于提供一種基于vr技術的空間定位定姿系統(tǒng)及其方法，通過利用激光傳感器融合imu慣性傳感器測量數(shù)據(jù)，采用基于卡爾曼濾波(最優(yōu)估計)的傳感器融合算法，實現(xiàn)系統(tǒng)連續(xù)正常輸出姿態(tài)與位置信息，從而解決應用激光測距原理進行測量過程存在遮擋的情況下不能連續(xù)正常輸出位置數(shù)據(jù)的技術缺陷。

為達到上述目的，本發(fā)明的技術方案是這樣實現(xiàn)的：

一種基于虛擬現(xiàn)實vr技術的空間定位定姿系統(tǒng)，包括vr頭盔，還包括兩個基站、兩只vr手柄以及一臺計算機；所述兩只vr手柄與vr頭盔之間，基站與vr頭盔之間，以及所述頭盔與所述計算機之間均通過藍牙模塊進行無線通信。

其中，所述基站還包括x軸電機驅動控制電路、y軸電機驅動控制電路、激光器驅動控制電路、led驅動控制電路、光敏傳感器接收調(diào)理電路以及藍牙通信模塊；

所述x軸、y軸電機驅動控制電路，分別用于驅動步進電機沿著x軸及y軸在一定角度范圍內(nèi)執(zhí)行周期性往復式的掃描動作；

所述激光器驅動電路，用于驅動激光器發(fā)射激光掃描信號，以形成用于被測載體的姿態(tài)和位置測量的激光掃描場；

led驅動控制電路，控制led按照固定周期閃爍發(fā)光，與被測載體上安裝的led光敏傳感器結合，用于觸發(fā)被測載體啟動計時、計算姿態(tài)和位置解算需要的時間參數(shù)；

光敏傳感器接收調(diào)理電路，用于調(diào)理激光信號與led信號，并將其轉化為適合下一環(huán)節(jié)測控單元接口的信號格式，還用于實現(xiàn)基站之間的通信功能并完成參考坐標系的建立；

藍牙通信模塊，用于實現(xiàn)所述基站與vr頭盔之間的無線通信功能。

所述vr頭盔，還包括左眼屏幕顯示電路、右眼屏幕顯示電路、高清視頻處理電路、光敏傳感器接收調(diào)理電路、usb通信電路以及藍牙通信模塊；其中：

左、右眼屏幕顯示電路，分別用于驅動左、右oled屏幕的點亮和正常畫面顯示；

高清視頻處理電路，用于接收和處理從所述計算機通過hdmi線傳輸過來的視頻信號；

光敏傳感器接收調(diào)理電路，用于調(diào)理激光信號與led信號，并將其轉化為適合下一環(huán)節(jié)測控單元接口的信號格式；

usb通信電路，用于所述計算機與vr頭盔之間的控制信號的通信傳輸；

藍牙通信模塊，用于實現(xiàn)所述vr手柄與vr頭盔、所述vr頭盔與所述基站之間的無線通信功能。

所述vr手柄，還包括按鍵處理電路、觸摸面板處理電路、光敏傳感器接收調(diào)理電路以及藍牙通信模塊；其中：

所述按鍵處理電路，用于接收和解碼vr手柄上的鍵位控制信號輸入；

觸摸面板處理電路，用于接收和解碼vr手柄上的觸摸板的控制信號輸入；

光敏傳感器接收調(diào)理電路，用于調(diào)理激光信號與led信號，并將其轉化為適合下一環(huán)節(jié)測控單元接口的信號格式；以及在所述vr手柄中的作用是用于實現(xiàn)對vr手柄的姿態(tài)和位置測量；

藍牙通信模塊，用于實現(xiàn)所述vr手柄與vr頭盔之間的無線通信功能。

所述計算機為支持圖形處理的主機。

一種基于虛擬現(xiàn)實vr技術的空間定位定姿方法，包括如下步驟：

a、led陣列發(fā)出光信號，頭盔上的光敏傳感器接收到光信號后，頭盔中的單片機開始計時；

b、當led閃光第一預設時長后，x軸的激光掃描器開始掃描，頭盔上的光敏傳感器被掃到時，記錄當前的計時時刻；

c、當led閃光第二預設時長后，y軸的激光掃描器開始掃描，頭盔上的光敏傳感器被掃到時，記錄當前的計時時刻；

d、依據(jù)掃描電機的轉速、光敏傳感器計時的時間間隔計算出角度，該角度為光敏傳感器在掃描坐標系下的x軸和y軸偏角；

e、依據(jù)光敏傳感器在頭盔上的位置是確定的，依據(jù)多個傳感器的位置差即可確定頭盔的位置和姿態(tài)。

其中，所述的第一預設時長等于第二預設時長。

步驟d所述光敏傳感器在掃描坐標系下的x軸和y軸偏角為電機轉速×時間間隔。

步驟e所述依據(jù)多個傳感器的位置差確定頭盔的位置和姿態(tài)的過程，進一步包括如下步驟：

a、設頭盔坐標系{h}到基站坐標系{b1}的齊次變換矩陣為：

其中，px、py、pz是頭盔坐標系原點在基站坐標系的位置，是頭盔坐標系在基站坐標系的旋轉矩陣，代表該頭盔的姿態(tài)；

b、設光敏傳感器為空間中一質點p1，其在頭盔坐標系的位置是px1、py1、pz1，則存在如下數(shù)學關系：

其中，是光敏傳感器p1在基站坐標系的位置；所述公式(2)的物理意義為空間中某個質點在兩個不同坐標系下的坐標間的變換關系；

c、依據(jù)掃描測量角度的原理還有如下的關系：

其中，tanθy是y軸的掃描測量角度，tanθx是x軸的掃描測量角度；所述公式(3)的物理意義為繞x軸掃描平面與繞y軸掃描平面的交線，即穿過基站坐標系原點和光敏傳感器的一條空間直線；

d、當有多個光敏傳感器被掃描到，根據(jù)公式(1)～(3)建立非線性方程組采用最小二乘法求最優(yōu)解即可確定頭盔坐標系位置和姿態(tài)。

一種應用于所述空間定位定姿方法的基于融合激光傳感器與慣性傳感器的測量方法，包括如下步驟：

利用卡爾曼濾波器對慣性測量系統(tǒng)、激光測量系統(tǒng)這兩種測量方式獲得的測量結果的步驟；

采用反饋校正的間接法估計，將慣導系統(tǒng)導航參數(shù)誤差的估值反饋到慣導系統(tǒng)內(nèi)，對誤差狀態(tài)進行校正的步驟；

對多傳感器的測量數(shù)據(jù)進行融合的步驟。

本發(fā)明所提供的基于虛擬現(xiàn)實(vr)技術的空間定位定姿系統(tǒng)及其方法，具有以下優(yōu)點：

1)本發(fā)明對載體位置和姿態(tài)信息的解算僅使用到時間參數(shù)，不涉及到圖像處理，對于位置的計算在設備本地就可以完成。并且直接將位置數(shù)據(jù)傳輸?shù)诫娔X上，省略了從攝像頭到電腦的高數(shù)據(jù)傳輸?shù)牟襟E。故而系統(tǒng)整體計算量小且功耗極低。

2)本發(fā)明采用了帶有線性激光器的轉臺發(fā)射機，所述兩個激光發(fā)射轉臺以穩(wěn)定的節(jié)拍對空間進行水平和豎直的光束掃描，激光會觸發(fā)vr手柄和vr頭盔上的光敏傳感器，通過測量光敏傳感器被觸發(fā)的時間計算出vr手柄與vr頭盔位置和姿態(tài)。與傳統(tǒng)的測距定位原理(如gps定位)相比，用這種方法只需要2臺激光發(fā)射機就可完成測量任務。

3)位置與姿態(tài)的解算使用的是時間參數(shù)，不涉及到圖像處理，對于位置的計算在設備本地即可完成，簡化了數(shù)據(jù)處理過程。

4)本定位系統(tǒng)數(shù)據(jù)傳輸方法是直接將位置數(shù)據(jù)傳輸?shù)诫娔X上，省略了從攝像頭到電腦的高數(shù)據(jù)傳輸?shù)牟襟E，簡化了數(shù)據(jù)信息處理過程，加快了數(shù)據(jù)傳輸速度。

5)本系統(tǒng)多傳感器數(shù)據(jù)融合方法運用了卡爾曼濾波(最優(yōu)估計)的傳感器融合算法，解決了最小二乘數(shù)據(jù)融合方法不能顯示考慮測量不確定性等問題，可獲得更高精度的曲面。

6)本系統(tǒng)采用基于快速卡爾曼濾波(最優(yōu)估計)的傳感器融合算法解決了激光傳感器激光掃描信號可能被遮擋的問題，實現(xiàn)位置數(shù)據(jù)信息數(shù)據(jù)的連續(xù)平滑輸出，提高了定位系統(tǒng)的性能。

附圖說明

圖1為本發(fā)明實施例基于虛擬現(xiàn)實(vr)技術系統(tǒng)的空間定位定姿系統(tǒng)組成示意圖；

圖2為本發(fā)明實施例的所述空間定位定姿系統(tǒng)的通信連接拓撲示意圖；

圖2a為所述基站的硬件結構示意圖；

圖2b為所述vr頭盔的硬件結構示意圖；

圖2c為所述手持式控制器(即vr手柄)的硬件結構示意圖；

圖3為本發(fā)明實施例的空間定位定姿系統(tǒng)進行空間定位定姿的方法流程示意圖；

圖3a為vr頭盔中的單片機開始計時的狀態(tài)示意圖；

圖3b為x軸的激光掃描器開始工作vr頭盔的光敏傳感器被掃描到時記錄當前時刻的狀態(tài)示意圖；

圖3c為y軸的激光掃描器開始工作vr頭盔的光敏傳感器被掃描到時記錄當前時刻的狀態(tài)示意圖；

圖3d為依據(jù)掃描電機的轉速和光敏傳感器計時的時間間隔計算偏角角度的狀態(tài)示意圖；

圖4為基站坐標系與頭盔坐標系關系示意圖；

圖4a為本發(fā)明實施例采用的計算原理示意圖；

圖5為所述vr頭盔的三維軌跡仿真結果示意圖；

圖6為坐標系中xyz各軸分量運動軌跡仿真結果示意圖；

圖7為各方位角運動軌跡仿真結果示意圖。

具體實施方式

下面結合附圖及本發(fā)明的實施例對本發(fā)明基于虛擬現(xiàn)實(vr)技術的空間定位定姿系統(tǒng)及其方法作進一步詳細的說明。

圖1為本發(fā)明實施例基于虛擬現(xiàn)實(vr)技術系統(tǒng)的空間定位定姿系統(tǒng)組成示意圖。

如圖1所示，該空間定位定姿系統(tǒng)包括：2臺線性激光器的轉臺發(fā)射機(又稱激光基站或基站)、2件手持虛擬現(xiàn)實控制器(即vr手柄)、頭戴式顯示器(即vr頭盔)、支持圖形處理的高性能的計算機和藍牙通信模塊。

每個基站里設有一個紅外led陣列，兩個轉軸互相垂直的旋轉的紅外激光發(fā)射器，轉速為10ms/圈。

基站的工作狀態(tài)：每20ms為一個掃描周期，在掃描周期開始時紅外led閃光，前10ms內(nèi)x軸的旋轉激光掃過用戶自由活動區(qū)域，y軸不掃光；下一個10ms內(nèi)y軸的旋轉激光掃過用戶自由活動區(qū)域，x軸不掃光。

一對手持虛擬現(xiàn)實控制器(即vr手柄)及頭戴式顯示器(即vr頭盔)，vr手柄及vr頭盔外部均設有多個激光光敏傳感器，在基站的led閃光之后就會進行信號同步，然后光敏傳感器可以測量出x軸激光和y軸激光分別到達傳感器的時間。這個時間就正好是x軸和y軸激光轉到這個特定的、點亮傳感器的角度的時間，于是傳感器相對于基站的x軸和y軸角度也就已知了。分布在vr頭盔和vr手柄上的光敏傳感器的位置也是已知的，于是通過各個傳感器的位置差，就可以計算出vr頭盔的位置和運動軌跡。

所述的vr手柄及頭盔內(nèi)部安裝有九軸imu慣性傳感器模塊，imu慣性傳感器模塊內(nèi)部包含微陀螺儀及微加速度計，用于采集vr手柄和頭盔的運動姿態(tài)信息，包括角速度，加速度、地心加速度方向等。

圖2為本發(fā)明實施例的所述空間定位定姿系統(tǒng)的通信連接拓撲示意圖。如圖2所示，各模塊之間均以低功耗的無線通信方式進行通信(優(yōu)選通過藍牙通信模塊進行無線通信)，即兩只vr手柄與vr頭盔之間，基站與vr頭盔之間，以及所述頭盔與計算機之間均通過藍牙模塊進行無線通信。

圖2a為所述基站的硬件結構示意圖。如圖2a所示，該基站主要包括供電電路、x軸電機驅動控制電路、y軸電機驅動控制電路、激光器驅動控制電路、led驅動控制電路、光敏傳感器接收調(diào)理電路以及藍牙通信模塊。其中：

供電電路，用于為系統(tǒng)各個電路和通信模塊提供穩(wěn)定的直流供電，包括電機、激光器以及其它功能芯片，同時還用于穩(wěn)定系統(tǒng)的電壓以及進行直流工作電壓轉換的功能。

x軸、y軸電機驅動控制電路，分別用于驅動步進電機沿著x軸及y軸在一定角度范圍內(nèi)執(zhí)行周期性往復式的掃描動作。

激光器驅動電路，用于驅動激光器發(fā)射激光掃描信號，從而形成激光掃描場，用于被測載體的姿態(tài)和位置測量。

led驅動控制電路，控制led按照固定周期閃爍發(fā)光，與被測載體上安裝的led光敏傳感器結合，用于觸發(fā)被測載體啟動計時，用于計算姿態(tài)和位置解算需要的時間參數(shù)。

光敏傳感器接收調(diào)理電路，用于調(diào)理激光信號與led信號，并將其轉化為適合下一環(huán)節(jié)測控單元接口的信號格式，在本基站的各電路和模塊中的功能是實現(xiàn)基站之間的通信功能并完成參考坐標系的建立。

藍牙通信模塊，用于實現(xiàn)所述基站與vr頭盔之間的無線通信功能。

圖2b為所述vr頭盔的硬件結構示意圖。如圖2b所示，該vr頭盔主要包括供電電路，左眼屏幕顯示電路、右眼屏幕顯示電路、高清視頻處理電路、光敏傳感器接收調(diào)理電路、相機處理電路、hdmi通信電路、音頻通信電路、usb通信電路以及藍牙通信模塊。其中：

供電電路，用于為系統(tǒng)各個功能電路和模塊提供穩(wěn)定的直流供電，包括oled屏幕、各個處理電路以及各個功能芯片，同時還用于穩(wěn)定系統(tǒng)電壓以及進行直流工作電壓轉換的功能。

左、右眼屏幕顯示電路，分別用于驅動左、右oled屏幕的點亮和正常畫面顯示。

高清視頻處理電路，用于接收和處理從pc主機(即所述計算機)通過hdmi線傳輸過來的視頻信號。

光敏傳感器接收調(diào)理電路，用于調(diào)理激光信號與led信號，并將其轉化為適合下一環(huán)節(jié)測控單元接口的信號格式。

較佳地，還包括相機處理電路，用于處理從前置攝像頭采集的畫面信號，還用于支持未來對vr頭盔擴展功能的使用。

hdmi通信電路，用于接收pc主機渲染完成的音視頻信號。

音頻通信電路，用于解碼hdmi通信電路接收到的立體音頻信號并輸出。

usb通信電路，用于pc主機與vr頭盔之間的控制信號的通信傳輸。

藍牙通信模塊，用于實現(xiàn)手持控制器與vr頭盔、所述vr頭盔與所述基站之間的無線通信功能。

圖2c為所述手持式控制器(即vr手柄)的硬件結構示意圖。如圖2c所示，該vr手柄主要包括供電電路、按鍵處理電路、觸摸面板處理電路、光敏傳感器接收調(diào)理電路以及藍牙通信模塊。其中：

供電電路，用于為系統(tǒng)各個電路和模塊提供穩(wěn)定的直流供電，包括光敏傳感器、各個處理電路模塊以及各個功能芯片，同時還用于穩(wěn)定系統(tǒng)電壓以及進行直流工作電壓轉換的功能。

按鍵處理電路，用于接收和解碼手持控制器即vr手柄上的鍵位控制信號輸入。

觸摸面板處理電路，用于接收和解碼控制器上的觸摸板的控制信號輸入。

光敏傳感器接收調(diào)理電路，用于調(diào)理激光信號與led信號，并將其轉化為適合下一環(huán)節(jié)測控單元接口的信號格式，在本手持控制器模塊中的功能是實現(xiàn)對控制器的姿態(tài)和位置測量。

藍牙通信模塊，用于實現(xiàn)手持控制器即vr手柄與vr頭盔之間的無線通信功能。

圖3為本發(fā)明實施例的空間定位定姿系統(tǒng)進行空間定位定姿的方法流程示意圖。

為了實時地確定vr頭盔和手持式控制器即vr手柄的位置和姿態(tài)，如圖3所示，所述的定位過程分為如下幾個步驟：

步驟31、led陣列發(fā)出光信號，vr頭盔上的光敏傳感器接收到光信號后，頭盔中的單片機開始計時。如圖3a所示。

步驟32、當led閃光第一預設時長(假設10ms)后，x軸的激光掃描器開始掃描，vr頭盔上的光敏傳感器被掃到時，記錄當前的計時時刻。如圖3b所示。

步驟33、當led閃光第二預設時長(假設10ms)后，y軸的激光掃描器開始掃描，頭盔上的光敏傳感器被掃到時，記錄當前的計時時刻。如圖3c所示。

這里，所述的第一預設時長等于第二預設時長，可根據(jù)實際想要達到的定位、定姿精度的需要設定為5ms、10ms、20ms等。

步驟34、依據(jù)掃描電機的轉速、光敏傳感器計時的時間間隔計算出角度，該角度為光敏傳感器在掃描坐標系下的x軸和y軸偏角。

如圖3d所示，所述偏角等于電機轉速×時間間隔。具體過程如下：led燈觸發(fā)計時，激光線光源開始啟動掃描，光敏傳感器接收到激光信號時計時結束，在這個計時周期電機的轉速是固定的，這樣用這電機轉速乘以這個時間間隔，就等于偏角。上述計算偏角的方法，對x軸、y軸原理相同。

步驟35、依據(jù)光敏傳感器在頭盔上的位置是確定的，依據(jù)多個傳感器的位置差即可確定頭盔的位置和姿態(tài)。

由于激光具有直線傳播特性，在本實施例中激光掃描信號存在被遮擋的情況下，由于最小二乘數(shù)據(jù)融合方法不能顯式考慮測量的不確定性等問題，所以發(fā)明人提出了基于卡爾曼濾波(最優(yōu)估計)的傳感器數(shù)據(jù)融合算法用于解決位置信息數(shù)據(jù)的連續(xù)平滑輸出。

下面為步驟35所述依據(jù)多個傳感器的位置差確定頭盔的位置和姿態(tài)的求解過程：

由于光敏傳感器在頭盔上的位置是確定的，要被定位的并非光敏傳感器本身，而是頭盔，光敏傳感器僅是定位頭盔位置和姿態(tài)的媒介。對于室內(nèi)定位而言，未知量是頭盔坐標系到基站坐標系的齊次變換矩陣。

假設頭盔坐標系{h}到基站坐標系{b1}的齊次變換矩陣為：

其中，px、py、pz是頭盔坐標系原點在基站坐標系的位置，是頭盔坐標系在基站坐標系的旋轉矩陣，它表征該頭盔的姿態(tài)。

光敏傳感器p1在頭盔坐標系的位置是確定且不變的(它由頭盔的結構外形設計保證)。設光敏傳感器可以看成空間中一個質點p1，它在頭盔坐標系的位置是px1、py1、pz1，則存在如下數(shù)學關系：

其中，是光敏傳感器p1在基站坐標系的位置。

該公式(2)的物理意義為：空間中某個質點在兩個不同坐標系下的坐標間的變換關系。

例如，對于某個光敏傳感器p1，當在頭盔坐標系下觀測它時，它的坐標是px1、py1、pz1；當在基站坐標系下觀測它時，它的坐標為而兩個坐標系之間的關系為這個變換矩陣中的元素px、py、pz就是頭盔位置，就是頭盔的姿態(tài)，定位算法的目的就是求解中的各元素。如圖4所示。

另外，依據(jù)掃描測量角度的原理還有如下的關系：

其中，tanθy是y軸的掃描測量角度，tanθx是x軸的掃描測量角度。

該公式(3)的物理意義為：繞x軸掃描平面與繞y軸掃描平面的交線，即穿過基站坐標系原點和光敏傳感器的一條空間直線。

如果有多個光敏傳感器被掃描到，那么根據(jù)公式(1)～(3)就可以建立非線性方程組求解位置和姿態(tài)。理論上如果有3個光敏傳感器被掃描到，就可以測量出位置和姿態(tài)，實際中被掃描到的點的數(shù)量遠大于3個，因此采用最小二乘法求最優(yōu)解即可確定頭盔坐標系位置和姿態(tài)。

本發(fā)明的上述實施例中，采用的基于融合激光傳感器與慣性傳感器的測量方法，能夠達到如下系統(tǒng)性能：定位范圍：最大可達5米×5米×3米；定位精度小于5毫米；系統(tǒng)延遲低于5毫秒。

在本空間定位定姿系統(tǒng)中，由于存在激光掃描信號被遮擋的情況，針對這種情況，我們提出了基于卡爾曼濾波原理的多傳感器測量數(shù)據(jù)融合的方法來解決位置與姿態(tài)信息數(shù)據(jù)的連續(xù)平滑輸出問題。所述卡爾曼濾波(器)原理是采用最優(yōu)自回歸數(shù)據(jù)處理算法，對多個傳感器的數(shù)據(jù)進行融合。

如圖4a所示的本發(fā)明實施例具體采用的計算原理示意圖，利用卡爾曼濾波器對慣性測量系統(tǒng)、激光測量系統(tǒng)這兩種測量方式獲得的測量結果做最優(yōu)估計。

本實施例中具體采用反饋校正的間接法估計，將慣導系統(tǒng)導航參數(shù)誤差的估值反饋到慣導系統(tǒng)內(nèi)，對誤差狀態(tài)進行校正。

系統(tǒng)方程和量測方程為：

校正過程為：

其中：是測量位置和姿態(tài)的最優(yōu)估計結果。

其中，多傳感器的測量數(shù)據(jù)融合過程可用下列公式來表征：

已知：

經(jīng)過多傳感器的測量數(shù)據(jù)融合后，可得到：

pt|t＝(i-ktht)pt|t-1。

其中，為室內(nèi)靶標位姿量測值；kt為室內(nèi)靶標位姿量測誤差；pt|t為室內(nèi)靶標位姿估計值。

圖5～圖7為本發(fā)明的實施例在matlab環(huán)境下的實驗數(shù)據(jù)仿真結果示意圖。其中，圖5為所述vr頭盔的三維軌跡仿真結果示意圖；圖6為坐標系中xyz各軸分量運動軌跡仿真結果示意圖；圖7為各方位角運動軌跡仿真結果示意圖。

從圖5～圖7所示的仿真結果可以直觀地觀察vr頭盔的實際位置與姿態(tài)數(shù)據(jù)的解算結果，從而驗證了利用本發(fā)明實施例中所述的空間定位定姿系統(tǒng)及其方法，是可以準確且連續(xù)地確定頭盔坐標系中被測目標的姿態(tài)和位置的。

以上所述，僅為本發(fā)明的較佳實施例而已，并非用于限定本發(fā)明的保護范圍。