陀螺儀空間定位方法

1、(19)中華人民共和國(guó)國(guó)家知識(shí)產(chǎn)權(quán)局*CN101881617A*(10)申請(qǐng)公布號(hào) CN 101881617 A(43)申請(qǐng)公布日 2010.11.10(12)發(fā)明專利申請(qǐng)(21)申請(qǐng)?zhí)?200910057180.6(22)申請(qǐng)日 2009.05.06(71)申請(qǐng)人鼎億數(shù)碼科技(上海)有限公司地址201204 上海市浦東新區(qū)張江高科技園區(qū)張衡路198弄10號(hào)401室申請(qǐng)人鼎億公司(72)發(fā)明人喻應(yīng)東(74)專利代理機(jī)構(gòu)上海浦一知識(shí)產(chǎn)權(quán)代理有限公司 31211代理人王江富(51)Int.Cl.G01C 21/00(2006.01)G01C 21/08(2006.01)G01C 21/10(200

2、6.01)G06F 3/038(2006.01)權(quán)利要求書(shū) 1 頁(yè) 說(shuō)明書(shū) 4 頁(yè) 附圖 4 頁(yè)(54)發(fā)明名稱陀螺儀空間定位方法(57)摘要本發(fā)明公開(kāi)了一種陀螺儀空間定位方法，在利用陀螺儀的相對(duì)位置定位的基礎(chǔ)上，通過(guò)引入磁傳感器獲得俯仰角和平面指向角，其中俯仰角也可以通過(guò)加速度傳感器獲得，補(bǔ)償陀螺儀相對(duì)位移偏差引起的實(shí)際指向偏差，從而進(jìn)行絕對(duì)位置定位，提高定位系統(tǒng)的指向性。CN 101881617 ACN 101881617 A權(quán) 利 要 求 書(shū)1/1頁(yè)1.一種陀螺儀空間定位方法，運(yùn)動(dòng)控制器件中固定有陀螺儀，其特征在于，運(yùn)動(dòng)控制器件中還固定有三軸磁傳感器，或二軸磁傳感器和加速度傳感器，包括以

3、下步驟；一.定位系統(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所述的陀螺儀空間定位方法，其特

4、征在于，保存步驟五獲得的運(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ù)

5、進(jìn)行穩(wěn)定性處理。CN 101881617 A說(shuō) 明 書(shū)陀螺儀空間定位方法1/4頁(yè)技術(shù)領(lǐng)域0001 本發(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í)。0003 現(xiàn)有的陀螺儀空間定位方法是采用陀螺儀定位方式，陀螺儀定位方式是基于角速度檢測(cè)的原理而設(shè)計(jì)，通過(guò)一系列的運(yùn)算，得到的定位位置是相對(duì)于上次定位的一個(gè)相對(duì)值?，F(xiàn)有的陀螺儀空間定位方法雖然已經(jīng)使得空中指向定位成為可能，但由于得到的定位位置只是相對(duì)于上

6、次定位的一個(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)存在有偏差。0004 比如，教學(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ò)用

7、戶手中設(shè)備上的攝像頭拍攝前方的光點(diǎn)實(shí)現(xiàn)定位，這種方法實(shí)現(xiàn)的指向性好了許多，但設(shè)計(jì)復(fù)雜，系統(tǒng)需要幾個(gè)部分組成，同時(shí)還要保證屏幕前光源的位置和角度，而且容易受到外界光源的影響，并且很難通過(guò)一般的手段進(jìn)行濾除。0002發(fā)明內(nèi)容0005 本發(fā)明要解決的技術(shù)問(wèn)題是提供一種陀螺儀空間定位方法，能夠?qū)崿F(xiàn)定位同指向的一致。0006 為解決上述技術(shù)問(wèn)題，本發(fā)明的種陀螺儀空間定位方法，運(yùn)動(dòng)控制器件中固定有陀螺儀，其特征在于，運(yùn)動(dòng)控制器件中還固定有三軸磁傳感器，或二軸磁傳感器和加速度傳感器，包括以下步驟；0007 一.定位系統(tǒng)初始化；0008 二.初始化完成后，采集陀螺儀原始數(shù)據(jù)；0009 三.進(jìn)行定位算法，根據(jù)陀

8、螺儀原始數(shù)據(jù)及上一定位點(diǎn)位置數(shù)據(jù)計(jì)算出運(yùn)動(dòng)控制器件目前定位點(diǎn)相對(duì)于上一定位點(diǎn)的相對(duì)位移；0010 四.采集獲取三軸磁傳感器，或二軸磁傳感器和加速度傳感器的數(shù)據(jù)；0011 五.利用三軸磁傳感器，或二軸磁傳感器和加速度傳感器的組合得到的數(shù)據(jù)，計(jì)算出目前運(yùn)動(dòng)控制器件的俯仰角和平面指向角，獲得運(yùn)動(dòng)控制器件當(dāng)前定位點(diǎn)絕對(duì)位置的CN 101881617 A說(shuō) 明 書(shū)2/4頁(yè)相對(duì)位移誤差補(bǔ)償量；0012 六.根據(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ù)。0013 本發(fā)明的陀螺儀空間定位方法，還保存步驟五獲得的運(yùn)動(dòng)控

9、制器件當(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)行定位算法。0014 本發(fā)明的陀螺儀空間定位方法，在利用陀螺儀的相對(duì)位置定位的基礎(chǔ)上，通過(guò)引入磁傳感器獲得俯仰角和平面指向角(其中，俯仰角也可以通過(guò)加

10、速度傳感器獲得)補(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ì)造成較大的躍變。附圖說(shuō)明0015 下面結(jié)合附圖

11、及具體實(shí)施方式對(duì)本發(fā)明作進(jìn)一步詳細(xì)說(shuō)明。0016 圖1是現(xiàn)有的陀螺儀空間定位方法指向性存在一定的偏差示意圖；0017 圖2是本發(fā)明的陀螺儀空間定位方法一實(shí)施方式流程圖；0018 圖3是本發(fā)明的陀螺儀空間定位方法一實(shí)施方式磁傳感器俯仰角和平面指向角補(bǔ)償區(qū)域示意圖；0019 圖4是本發(fā)明的陀螺儀空間定位方法一實(shí)施方式引入磁傳感器進(jìn)行俯仰角和平面指向角補(bǔ)償示意圖；0020 圖5是三軸磁傳感器坐標(biāo)方向圖。具體實(shí)施方式0021 三軸磁傳感器坐標(biāo)方向如圖5所示，它的方向參考是地磁場(chǎng)，固定在運(yùn)動(dòng)控制器件中的三軸磁傳感器，當(dāng)運(yùn)動(dòng)控制器件的指向發(fā)生變化時(shí)，它都會(huì)相對(duì)于地磁場(chǎng)有一定的變化。如圖5所示，三軸磁傳感器

12、分別指向相互垂直的角度(如圖中所示的磁傳感器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)系如下：00220023 標(biāo)注：CN 101881617 A00240025002600270028 說(shuō) 明 書(shū)3/4頁(yè)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)行必

13、要的數(shù)據(jù)處理后，可以計(jì)算出平面指向角和俯仰角：00290030加速度傳感器可以為陀螺儀在垂直方向定位提供補(bǔ)償?shù)幕A(chǔ)是重力加速度的存在，并且重力加速度方向在某一固定位置時(shí)固定的。根據(jù)重力加速度在多軸加速度傳感器上的分量，計(jì)算出此時(shí)設(shè)備的俯仰角。0032 本發(fā)明的陀螺儀空間定位方法一實(shí)施方式如圖2所示，包括以下步驟：0033 1.運(yùn)動(dòng)控制器件中固定有陀螺儀、三軸磁傳感器(或二軸磁傳感器和加速度傳感器)，定位系統(tǒng)初始化；00312.初始化完成后，采集用于運(yùn)動(dòng)控制器件定位的兩軸陀螺儀(或三軸陀螺儀)原始數(shù)據(jù)；進(jìn)行陀螺儀原始數(shù)據(jù)的穩(wěn)定性處理，主要包含有濾波處理、多軸之間去交叉干擾處理、溫度補(bǔ)償處理等；0

14、035 3.進(jìn)行定位算法，根據(jù)陀螺儀原始數(shù)據(jù)及上一定位點(diǎn)位置數(shù)據(jù)計(jì)算出運(yùn)動(dòng)控制器件目前定位點(diǎn)相對(duì)于上一定位點(diǎn)的相對(duì)位移；0036 4.采集獲取三軸磁傳感器(或二軸磁傳感器和加速度傳感器)的數(shù)據(jù)，對(duì)這些數(shù)據(jù)作數(shù)據(jù)穩(wěn)定性處理；0037 5.利用三軸磁傳感器或兩軸磁傳感器與加速度傳感器的組合得到的數(shù)據(jù)，計(jì)算目前運(yùn)動(dòng)控制器件的俯仰角和平面指向角，獲得運(yùn)動(dòng)控制器件當(dāng)前定位點(diǎn)絕對(duì)位置的相對(duì)位移誤差補(bǔ)償量；0038 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ù)。0039 三軸磁傳感器(或二軸磁傳感器和加速度傳感

15、器)俯仰角和平面指向角補(bǔ)償區(qū)域如圖3所示；引入三軸磁傳感器(或二軸磁傳感器和加速度傳感器)進(jìn)行俯仰角和平面指向角補(bǔ)償后定位如圖4所示，定位同指向一致。0034本發(fā)明的陀螺儀空間定位方法，在利用陀螺儀的相對(duì)位置定位的基礎(chǔ)上，通過(guò)引入磁傳感器獲得俯仰角和平面指向角(其中，俯仰角也可以通過(guò)加速度傳感器獲得)，補(bǔ)償由陀螺儀相對(duì)位移偏差引起的實(shí)際指向偏差，從而進(jìn)行絕對(duì)位置定位，提高定位系統(tǒng)的指向性。0041 但絕對(duì)位置定位與相對(duì)位置定位在一定程度上存在某些矛盾，比如，當(dāng)陀螺儀的相對(duì)位移定位與三軸磁場(chǎng)傳感器(或二軸磁場(chǎng)傳感器與加速度傳感器)絕對(duì)定位之間產(chǎn)生較大的差異時(shí)，如果這時(shí)引入三軸磁場(chǎng)傳感器(或二軸磁

16、場(chǎng)傳感器和加速度傳感器)的指向補(bǔ)償，可能會(huì)造成定位點(diǎn)的跳躍，從而導(dǎo)致定位的穩(wěn)定性較差。0040CN 101881617 A0042 說(shuō) 明 書(shū)4/4頁(yè)為了提高定位的穩(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)行定位算法。0043 本發(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ù)