專利名稱：陀螺儀空間定位方法
技術(shù)領(lǐng)域：
本發(fā)明涉及定位技術(shù)，特別涉及一種陀螺儀空間定位方法。
背景技術(shù)：
目前，隨著MEMS (微機(jī)電系統(tǒng))技術(shù)在消費(fèi)類電子產(chǎn)品中廣泛的應(yīng)用，涌現(xiàn)出許多 新生電子產(chǎn)物，比如空中鼠標(biāo)、模仿人體動(dòng)作的游戲手柄等。陀螺儀與加速度傳感器的結(jié) 合，使得類似空中鼠標(biāo)等產(chǎn)品成為現(xiàn)實(shí)?，F(xiàn)有的陀螺儀空間定位方法是采用陀螺儀定位方式，陀螺儀定位方式是基于角速 度檢測(cè)的原理而設(shè)計(jì)，通過(guò)一系列的運(yùn)算，得到的定位位置是相對(duì)于上次定位的一個(gè)相對(duì) 值?，F(xiàn)有的陀螺儀空間定位方法雖然已經(jīng)使得空中指向定位成為可能，但由于得到的定位 位置只是相對(duì)于上次定位的一個(gè)相對(duì)值，指向性存在一定的偏差，當(dāng)長(zhǎng)時(shí)間偏差得不到補(bǔ) 償時(shí)，偏差就會(huì)在一定程度上進(jìn)行累加，以至于操作的體驗(yàn)越來(lái)越差，在某些特定場(chǎng)合，它 還不能完全滿足需要。如圖1所示，現(xiàn)有的陀螺儀空間定位方法，實(shí)際的定位點(diǎn)2同期望定 位點(diǎn)存在有偏差。比如，教學(xué)用的空中鼠標(biāo)中，當(dāng)教師需要指出屏幕上顯示的教案的一部分，有可 能鼠標(biāo)的光標(biāo)已經(jīng)移到了待指定對(duì)象，而手的指向并沒(méi)有指向該對(duì)象，而當(dāng)手指向該對(duì)象 后，光標(biāo)又指向了其他的地方；又如，在射擊游戲中，經(jīng)常會(huì)覺(jué)得手的指向和射擊的對(duì)象不 一致，從而使用戶的體驗(yàn)變差，常常需要有一個(gè)重新定位的處理(比如通過(guò)按鍵觸發(fā)定位 等)。雖然通過(guò)在屏幕前放置一定的光源，通過(guò)用戶手中設(shè)備上的攝像頭拍攝前方的光點(diǎn)實(shí) 現(xiàn)定位，這種方法實(shí)現(xiàn)的指向性好了許多，但設(shè)計(jì)復(fù)雜，系統(tǒng)需要幾個(gè)部分組成，同時(shí)還要 保證屏幕前光源的位置和角度，而且容易受到外界光源的影響，并且很難通過(guò)一般的手段 進(jìn)行濾除。

發(fā)明內(nèi)容
本發(fā)明要解決的技術(shù)問(wèn)題是提供一種陀螺儀空間定位方法，能夠?qū)崿F(xiàn)定位同指向
的一致。為解決上述技術(shù)問(wèn)題，本發(fā)明的種陀螺儀空間定位方法，運(yùn)動(dòng)控制器件中固定有 陀螺儀，其特征在于，運(yùn)動(dòng)控制器件中還固定有三軸磁傳感器，或二軸磁傳感器和加速度傳 感器，包括以下步驟；一 .定位系統(tǒng)初始化；二 .初始化完成后，采集陀螺儀原始數(shù)據(jù)；三.進(jìn)行定位算法，根據(jù)陀螺儀原始數(shù)據(jù)及上一定位點(diǎn)位置數(shù)據(jù)計(jì)算出運(yùn)動(dòng)控制 器件目前定位點(diǎn)相對(duì)于上一定位點(diǎn)的相對(duì)位移；四.采集獲取三軸磁傳感器，或二軸磁傳感器和加速度傳感器的數(shù)據(jù)；五.利用三軸磁傳感器，或二軸磁傳感器和加速度傳感器的組合得到的數(shù)據(jù)，計(jì) 算出目前運(yùn)動(dòng)控制器件的俯仰角和平面指向角，獲得運(yùn)動(dòng)控制器件當(dāng)前定位點(diǎn)絕對(duì)位置的相對(duì)位移誤差補(bǔ)償量；六.根據(jù)計(jì)算獲得的相對(duì)位移誤差補(bǔ)償量，補(bǔ)償步驟三得到的運(yùn)動(dòng)控制器件目前 定位點(diǎn)相對(duì)于上一定位點(diǎn)的相對(duì)位移，獲得運(yùn)動(dòng)控制器件當(dāng)前定位點(diǎn)絕對(duì)位置數(shù)據(jù)。本發(fā)明的陀螺儀空間定位方法，還保存步驟五獲得的運(yùn)動(dòng)控制器件當(dāng)前定位點(diǎn)絕 對(duì)位置的相對(duì)位移誤差補(bǔ)償量，同以前保存的相對(duì)位移誤差補(bǔ)償量組成一相對(duì)位移誤差補(bǔ) 償量曲線表，根據(jù)位移誤差補(bǔ)償量曲線表的相對(duì)位移誤差補(bǔ)償量變化規(guī)律，確定誤差補(bǔ)償 量變化的趨勢(shì)，計(jì)算產(chǎn)生出定位相對(duì)誤差，作為對(duì)下次獲得的陀螺儀原始數(shù)據(jù)進(jìn)行數(shù)據(jù)修 正的相對(duì)位移前端補(bǔ)償數(shù)據(jù)，在對(duì)下次采集的陀螺儀原始數(shù)據(jù)進(jìn)行定位算法之前，先對(duì)下 次采集的陀螺儀原始數(shù)據(jù)進(jìn)行數(shù)據(jù)修正，提前補(bǔ)償相對(duì)位移誤差，然后再根據(jù)進(jìn)行數(shù)據(jù)修 正后的陀螺儀原始數(shù)據(jù)及上一定位點(diǎn)位置數(shù)據(jù)進(jìn)行定位算法。本發(fā)明的陀螺儀空間定位方法，在利用陀螺儀的相對(duì)位置定位的基礎(chǔ)上，通過(guò)引 入磁傳感器獲得俯仰角和平面指向角(其中，俯仰角也可以通過(guò)加速度傳感器獲得)補(bǔ)償 由陀螺儀相對(duì)位移偏差引起的實(shí)際指向偏差，從而進(jìn)行絕對(duì)位置定位，提高定位系統(tǒng)的指 向性。還進(jìn)一步通過(guò)一段時(shí)間的相對(duì)位移誤差補(bǔ)償量的變化，分析出位移誤差補(bǔ)償量變化 的趨勢(shì)，計(jì)算產(chǎn)生出定位相對(duì)誤差，作為對(duì)下次采集的陀螺儀原始數(shù)據(jù)進(jìn)行數(shù)據(jù)修正的相 對(duì)位移前端補(bǔ)償數(shù)據(jù)，對(duì)下次采集的陀螺儀數(shù)據(jù)進(jìn)行數(shù)據(jù)修正，提前補(bǔ)償陀螺儀的相對(duì)位 移定位與三軸磁傳感器(或二軸磁傳感器和加速度傳感器)絕對(duì)定位之間產(chǎn)生的差異(相 對(duì)位移誤差)，這樣可以使相對(duì)定位的結(jié)果與絕對(duì)定位結(jié)果之間的偏差減小，這樣在進(jìn)行下 次三軸磁傳感器(或二軸磁傳感器和加速度傳感器)指向性補(bǔ)償時(shí)，不會(huì)造成較大的躍變。


下面結(jié)合附圖及具體實(shí)施方式
對(duì)本發(fā)明作進(jìn)一步詳細(xì)說(shuō)明。圖1是現(xiàn)有的陀螺儀空間定位方法指向性存在一定的偏差示意圖；圖2是本發(fā)明的陀螺儀空間定位方法一實(shí)施方式流程圖；圖3是本發(fā)明的陀螺儀空間定位方法一實(shí)施方式磁傳感器俯仰角和平面指向角 補(bǔ)償區(qū)域示意圖；圖4是本發(fā)明的陀螺儀空間定位方法一實(shí)施方式引入磁傳感器進(jìn)行俯仰角和平 面指向角補(bǔ)償示意圖；圖5是三軸磁傳感器坐標(biāo)方向圖。
具體實(shí)施例方式三軸磁傳感器坐標(biāo)方向如圖5所示，它的方向參考是地磁場(chǎng)，固定在運(yùn)動(dòng)控制器 件中的三軸磁傳感器，當(dāng)運(yùn)動(dòng)控制器件的指向發(fā)生變化時(shí)，它都會(huì)相對(duì)于地磁場(chǎng)有一定的 變化。如圖5所示，三軸磁傳感器分別指向相互垂直的角度(如圖中所示的磁傳感器X軸、 磁傳感器Y軸、磁傳感器Z軸)。實(shí)際地磁場(chǎng)的指向?yàn)閳D中標(biāo)示的“實(shí)際地磁場(chǎng)北極方向”。 當(dāng)運(yùn)動(dòng)控制器件指向任何方向時(shí)，地磁場(chǎng)產(chǎn)生的作用會(huì)分配在磁傳感器的三軸上，他們之 間的關(guān)系如下yjaxisX2 +axisY2 +axisZ2 = Cgnd標(biāo)注
4
axisX 地磁場(chǎng)向量在傳感器X軸上的分量；axisY 地磁場(chǎng)向量在傳感器Y軸上的分量；axisZ 地磁場(chǎng)向量在傳感器Z軸上的分量；Cgnd 地磁場(chǎng)向量目前作用于運(yùn)動(dòng)控制器件的磁場(chǎng)強(qiáng)度。axisX、axisY、axisZ是目前地磁傳感器三軸采集的磁場(chǎng)強(qiáng)度值，進(jìn)行必要的數(shù)據(jù)
處理后，可以計(jì)算出平面指向角a和俯仰角5
r ，axisYa 二 arctan-
axisX
^axisZo = arctan ,
V axisX2 + axisY2加速度傳感器可以為陀螺儀在垂直方向定位提供補(bǔ)償?shù)幕A(chǔ)是重力加速度的存 在，并且重力加速度方向在某一固定位置時(shí)固定的。根據(jù)重力加速度在多軸加速度傳感器 上的分量，計(jì)算出此時(shí)設(shè)備的俯仰角。本發(fā)明的陀螺儀空間定位方法一實(shí)施方式如圖2所示，包括以下步驟1.運(yùn)動(dòng)控制器件中固定有陀螺儀、三軸磁傳感器(或二軸磁傳感器和加速度傳感 器)，定位系統(tǒng)初始化；2.初始化完成后，采集用于運(yùn)動(dòng)控制器件定位的兩軸陀螺儀(或三軸陀螺儀)原 始數(shù)據(jù)；進(jìn)行陀螺儀原始數(shù)據(jù)的穩(wěn)定性處理，主要包含有濾波處理、多軸之間去交叉干擾處 理、溫度補(bǔ)償處理等；3.進(jìn)行定位算法，根據(jù)陀螺儀原始數(shù)據(jù)及上一定位點(diǎn)位置數(shù)據(jù)計(jì)算出運(yùn)動(dòng)控制器 件目前定位點(diǎn)相對(duì)于上一定位點(diǎn)的相對(duì)位移；4.采集獲取三軸磁傳感器(或二軸磁傳感器和加速度傳感器)的數(shù)據(jù)，對(duì)這些數(shù) 據(jù)作數(shù)據(jù)穩(wěn)定性處理；5.利用三軸磁傳感器或兩軸磁傳感器與加速度傳感器的組合得到的數(shù)據(jù)，計(jì)算目 前運(yùn)動(dòng)控制器件的俯仰角和平面指向角，獲得運(yùn)動(dòng)控制器件當(dāng)前定位點(diǎn)絕對(duì)位置的相對(duì)位 移誤差補(bǔ)償量；6.根據(jù)計(jì)算獲得的相對(duì)位移誤差補(bǔ)償量，補(bǔ)償步驟三得到的運(yùn)動(dòng)控制器件目前定 位點(diǎn)相對(duì)于上一定位點(diǎn)的相對(duì)位移，獲得運(yùn)動(dòng)控制器件當(dāng)前定位點(diǎn)絕對(duì)位置數(shù)據(jù)。三軸磁傳感器(或二軸磁傳感器和加速度傳感器)俯仰角和平面指向角補(bǔ)償區(qū)域 如圖3所示；引入三軸磁傳感器(或二軸磁傳感器和加速度傳感器)進(jìn)行俯仰角和平面指 向角補(bǔ)償后定位如圖4所示，定位同指向一致。本發(fā)明的陀螺儀空間定位方法，在利用陀螺儀的相對(duì)位置定位的基礎(chǔ)上，通過(guò)引 入磁傳感器獲得俯仰角和平面指向角(其中，俯仰角也可以通過(guò)加速度傳感器獲得)，補(bǔ)償 由陀螺儀相對(duì)位移偏差引起的實(shí)際指向偏差，從而進(jìn)行絕對(duì)位置定位，提高定位系統(tǒng)的指 向性。但絕對(duì)位置定位與相對(duì)位置定位在一定程度上存在某些矛盾，比如，當(dāng)陀螺儀的 相對(duì)位移定位與三軸磁場(chǎng)傳感器(或二軸磁場(chǎng)傳感器與加速度傳感器)絕對(duì)定位之間產(chǎn)生 較大的差異時(shí)，如果這時(shí)引入三軸磁場(chǎng)傳感器(或二軸磁場(chǎng)傳感器和加速度傳感器)的指 向補(bǔ)償，可能會(huì)造成定位點(diǎn)的跳躍，從而導(dǎo)致定位的穩(wěn)定性較差。
為了提高定位的穩(wěn)定性，本發(fā)明的陀螺儀空間定位方法，還進(jìn)一步保存步驟5獲 得的運(yùn)動(dòng)控制器件當(dāng)前定位點(diǎn)絕對(duì)位置的相對(duì)位移誤差補(bǔ)償量，同以前保存的相對(duì)位移誤 差補(bǔ)償量組成一相對(duì)位移誤差補(bǔ)償量曲線表，根據(jù)位移誤差補(bǔ)償量曲線表的相對(duì)位移誤差 補(bǔ)償量變化規(guī)律，確定誤差補(bǔ)償量變化的趨勢(shì)，計(jì)算產(chǎn)生出定位相對(duì)誤差，作為對(duì)下次獲得 的陀螺儀原始數(shù)據(jù)進(jìn)行數(shù)據(jù)修正的相對(duì)位移前端補(bǔ)償數(shù)據(jù)，在對(duì)下次采集的陀螺儀原始數(shù) 據(jù)進(jìn)行定位算法之前，先對(duì)下次采集的陀螺儀數(shù)據(jù)進(jìn)行數(shù)據(jù)修正，提前補(bǔ)償相對(duì)位移誤差， 然后再進(jìn)行定位算法。本發(fā)明的陀螺儀空間定位方法進(jìn)一步通過(guò)一段時(shí)間的相對(duì)位移誤差補(bǔ)償量的變 化，分析出位移誤差補(bǔ)償量變化的趨勢(shì)，計(jì)算產(chǎn)生出定位相對(duì)誤差，作為對(duì)下次采集的陀螺 儀原始數(shù)據(jù)進(jìn)行數(shù)據(jù)修正的相對(duì)位移前端補(bǔ)償數(shù)據(jù)，對(duì)下次采集的陀螺儀數(shù)據(jù)進(jìn)行數(shù)據(jù)修 正，提前補(bǔ)償陀螺儀的相對(duì)位移定位與磁場(chǎng)傳感器(或加速度傳感器)絕對(duì)定位之間產(chǎn)生 的差異(相對(duì)位移誤差)，這樣可以使得步驟3進(jìn)行定位算法計(jì)算出的相對(duì)定位的結(jié)果與步 驟6獲得的絕對(duì)定位結(jié)果之間的偏差減小，這樣在進(jìn)行下次磁傳感器(或加速度傳感器) 指向性補(bǔ)償時(shí)，不會(huì)造成較大的躍變。
權(quán)利要求
一種陀螺儀空間定位方法，運(yùn)動(dòng)控制器件中固定有陀螺儀，其特征在于，運(yùn)動(dòng)控制器件中還固定有三軸磁傳感器，或二軸磁傳感器和加速度傳感器，包括以下步驟；一.定位系統(tǒng)初始化；二.初始化完成后，采集陀螺儀原始數(shù)據(jù)；三.進(jìn)行定位算法，根據(jù)陀螺儀原始數(shù)據(jù)及上一定位點(diǎn)位置數(shù)據(jù)計(jì)算出運(yùn)動(dòng)控制器件目前定位點(diǎn)相對(duì)于上一定位點(diǎn)的相對(duì)位移；四.采集獲取三軸磁傳感器，或二軸磁傳感器和加速度傳感器的數(shù)據(jù)；五.利用三軸磁傳感器，或二軸磁傳感器和加速度傳感器的組合得到的數(shù)據(jù)，計(jì)算出目前運(yùn)動(dòng)控制器件的俯仰角和平面指向角，獲得運(yùn)動(dòng)控制器件當(dāng)前定位點(diǎn)絕對(duì)位置的相對(duì)位移誤差補(bǔ)償量；六.根據(jù)計(jì)算獲得的相對(duì)位移誤差補(bǔ)償量，補(bǔ)償步驟三得到的運(yùn)動(dòng)控制器件目前定位點(diǎn)相對(duì)于上一定位點(diǎn)的相對(duì)位移，獲得運(yùn)動(dòng)控制器件當(dāng)前定位點(diǎn)絕對(duì)位置數(shù)據(jù)。
2.根據(jù)權(quán)利要求1所述的陀螺儀空間定位方法，其特征在于，保存步驟五獲得的運(yùn)動(dòng) 控制器件當(dāng)前定位點(diǎn)絕對(duì)位置的相對(duì)位移誤差補(bǔ)償量，同以前保存的相對(duì)位移誤差補(bǔ)償量 組成一相對(duì)位移誤差補(bǔ)償量曲線表，根據(jù)位移誤差補(bǔ)償量曲線表的相對(duì)位移誤差補(bǔ)償量變 化規(guī)律，確定誤差補(bǔ)償量變化的趨勢(shì)，計(jì)算產(chǎn)生出定位相對(duì)誤差，作為對(duì)下次獲得的陀螺儀 原始數(shù)據(jù)進(jìn)行數(shù)據(jù)修正的相對(duì)位移前端補(bǔ)償數(shù)據(jù)，在對(duì)下次采集的陀螺儀原始數(shù)據(jù)進(jìn)行定 位算法之前，先對(duì)下次采集的陀螺儀原始數(shù)據(jù)進(jìn)行數(shù)據(jù)修正，提前補(bǔ)償相對(duì)位移誤差，然后 再根據(jù)進(jìn)行數(shù)據(jù)修正后的陀螺儀原始數(shù)據(jù)及上一定位點(diǎn)位置數(shù)據(jù)進(jìn)行定位算法。
3.根據(jù)權(quán)利要求1所述的陀螺儀空間定位方法，其特征在于，對(duì)采集的陀螺儀原始數(shù) 據(jù)、磁傳感器或加速度傳感器的數(shù)據(jù)進(jìn)行穩(wěn)定性處理。
全文摘要
本發(fā)明公開(kāi)了一種陀螺儀空間定位方法，在利用陀螺儀的相對(duì)位置定位的基礎(chǔ)上，通過(guò)引入磁傳感器獲得俯仰角和平面指向角，其中俯仰角也可以通過(guò)加速度傳感器獲得，補(bǔ)償陀螺儀相對(duì)位移偏差引起的實(shí)際指向偏差，從而進(jìn)行絕對(duì)位置定位，提高定位系統(tǒng)的指向性。
文檔編號(hào)G01C21/08GK101881617SQ200910057180
公開(kāi)日2010年11月10日 申請(qǐng)日期2009年5月6日 優(yōu)先權(quán)日2009年5月6日
發(fā)明者喻應(yīng)東 申請(qǐng)人:鼎億數(shù)碼科技(上海)有限公司;鼎億公司