陀螺儀輔助定位方法
【專(zhuān)利摘要】本發(fā)明公開(kāi)了一種陀螺儀輔助定位方法，包括以下步驟：首先、姿態(tài)角的測(cè)量:對(duì)加速度計(jì)的測(cè)量值和磁傳感器的測(cè)量值分別經(jīng)過(guò)高斯牛頓迭代得到姿態(tài)誤差速率,陀螺儀直接通過(guò)四元數(shù)微分方程得到姿態(tài)四元數(shù)速率,兩個(gè)加起來(lái)積分得到姿態(tài)四元數(shù)；其次、初始點(diǎn)的獲?。豪眉铀俣扔?jì)和陀螺儀芯片兩者的數(shù)據(jù)，進(jìn)行軌跡推算，以將軌跡周期提高到慣性測(cè)量單元的數(shù)據(jù)周期；第三、速度和移動(dòng)距離的計(jì)算；第四、計(jì)算位置點(diǎn)；第五：糾正位置。本發(fā)明針對(duì)性的解決了陀螺儀的漂移、載體的線性加速度和周?chē)植看艌?chǎng)的干擾問(wèn)題，根據(jù)道路數(shù)據(jù)，對(duì)定位結(jié)果進(jìn)行投影修正，并利用修正結(jié)果計(jì)算下一個(gè)位置點(diǎn)，這樣就有效地減小累計(jì)誤差。
【專(zhuān)利說(shuō)明】陀螺儀輔助定位方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及輔助定位方法，尤其是涉及一種脫落及輔助定位及其方法。
【背景技術(shù)】
[0002]現(xiàn)有的陀螺儀輔助導(dǎo)航方法是采用陀螺儀定位方式，陀螺儀定位方式是基于角速度檢測(cè)的原理而設(shè)計(jì)，通過(guò)一系列的運(yùn)算，得到的定位位置是相對(duì)于上次定位的一個(gè)相對(duì)值?，F(xiàn)有的陀螺儀輔助導(dǎo)航方法雖然已經(jīng)使得空中指向定位成為可能，但由于得到的定位位置只是相對(duì)于上次定位的一個(gè)相對(duì)值，指向性存在一定的偏差，當(dāng)長(zhǎng)時(shí)間偏差得不到補(bǔ)償時(shí)，偏差就會(huì)在一定程度上進(jìn)行累加，以至于體驗(yàn)越來(lái)越差，在某些特定場(chǎng)合，它還不能完全滿足需要，另外傳統(tǒng)的陀螺儀輔助導(dǎo)航是基于最后的位置點(diǎn)進(jìn)行導(dǎo)航，在導(dǎo)航過(guò)程中會(huì)因?yàn)闀r(shí)間的增加累計(jì)誤差不斷的增大。

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

[0003]本發(fā)明所要解決的技術(shù)問(wèn)題是針對(duì)上述現(xiàn)有技術(shù)的不足，提供一種精度高、誤差小的陀螺儀輔助定位方法。
[0004]為解決上述技術(shù)問(wèn)題，本發(fā)明采取的技術(shù)方案為:陀螺儀輔助定位方法，其特征在于包括以下步驟:
首先、姿態(tài)角的測(cè)量:對(duì)加速度計(jì)的測(cè)量值和磁傳感器的測(cè)量值分別經(jīng)過(guò)高斯牛頓迭代得到姿態(tài)誤差速率，陀螺儀直接通過(guò)四元數(shù)微分方程得到姿態(tài)四元數(shù)速率，兩個(gè)加起來(lái)積分得到姿態(tài)四元數(shù)；
其次、初始點(diǎn)的獲取:利用加速度計(jì)和陀螺儀芯片兩者的數(shù)據(jù)，進(jìn)行軌跡推算，以將軌跡周期提高到慣性測(cè)量單元的數(shù)據(jù)周期；
第三、速度和移動(dòng)距離的計(jì)算:根據(jù)修正過(guò)的歐拉角，計(jì)算三軸的加速度的分量，從而計(jì)算出實(shí)際的加速度；在已知初速度，加速度和時(shí)間的情況下，通過(guò)對(duì)初速度積分和加速度二次積分，計(jì)算出移動(dòng)距離；
第四、計(jì)算位置點(diǎn):根據(jù)移動(dòng)距離和方向計(jì)算當(dāng)前的位置點(diǎn)的經(jīng)緯度；
第五:糾正位置:根據(jù)計(jì)算的位置點(diǎn)，在道路上進(jìn)行投影，進(jìn)行平滑處理，得到糾正后的位置點(diǎn)。
[0005]陀螺儀的漂移、載體的線性加速度和周?chē)植看艌?chǎng)的干擾是制約MARG傳感器姿態(tài)測(cè)量精度的主要問(wèn)題。本發(fā)明基于已有的慣性測(cè)量單元，設(shè)計(jì)了一個(gè)基于四元數(shù)的Kalman濾波器，通過(guò)建立MARG傳感器模型，引入傳感器偏差補(bǔ)償和自適應(yīng)的測(cè)量噪聲協(xié)方差矩陣構(gòu)造方法來(lái)提高姿態(tài)測(cè)量精度，減小載體線性加速度和周?chē)植看艌?chǎng)的干擾，實(shí)現(xiàn)三自由度的姿態(tài)測(cè)量。傳統(tǒng)的陀螺儀輔助導(dǎo)航是基于最后的位置點(diǎn)進(jìn)行導(dǎo)航，在導(dǎo)航過(guò)程中會(huì)因?yàn)闀r(shí)間的增加累計(jì)誤差不斷的增大，本發(fā)明是根據(jù)道路數(shù)據(jù)，對(duì)定位結(jié)果進(jìn)行投影修正，并利用修正結(jié)果計(jì)算下一個(gè)位置點(diǎn)，這樣就有效地減小累計(jì)誤差?？傊?，現(xiàn)有技術(shù)沒(méi)有解決陀螺儀的漂移、載體的線性加速度和周?chē)植看艌?chǎng)的干擾和長(zhǎng)時(shí)間的累積誤差問(wèn)題，本發(fā)明針對(duì)性的解決了陀螺儀的漂移、載體的線性加速度和周?chē)植看艌?chǎng)的干擾問(wèn)題，并把陀螺儀的空間定位技術(shù)和實(shí)際的導(dǎo)航應(yīng)用相結(jié)合，解決了長(zhǎng)時(shí)間的累積誤差問(wèn)題，而且根據(jù)道路數(shù)據(jù)，對(duì)定位結(jié)果進(jìn)行投影修正，并利用修正結(jié)果計(jì)算下一個(gè)位置點(diǎn)，這樣就有效地減小累計(jì)誤差。
【專(zhuān)利附圖】

【附圖說(shuō)明】
[0006]圖1為姿態(tài)角測(cè)量的數(shù)學(xué)式圖。
[0007]圖2為慣性加速度矢量圖。
[0008]圖3為糾正后的位置點(diǎn)圖。
【具體實(shí)施方式】
[0009]實(shí)施例1
本陀螺儀輔助定位方法，包括以下步驟:
首先、姿態(tài)角的測(cè)量:對(duì)加速度計(jì)的測(cè)量值和磁傳感器的測(cè)量值分別經(jīng)過(guò)高斯牛頓迭代得到姿態(tài)誤差速率，陀螺儀直接通過(guò)四元數(shù)微分方程得到姿態(tài)四元數(shù)速率，兩個(gè)加起來(lái)積分得到姿態(tài)四元數(shù)。參見(jiàn)圖1，本陀螺儀輔助定位方法基于已有的慣性測(cè)量單元，設(shè)計(jì)了一個(gè)基于四元數(shù)的Kalman濾波器，通過(guò)建立MARG傳感器模型，引入傳感器偏差補(bǔ)償和自適應(yīng)的測(cè)量噪聲協(xié)方差矩陣構(gòu)造方法來(lái)提高姿態(tài)測(cè)量精度，減小載體線性加速度和周?chē)植看艌?chǎng)的干擾，實(shí)現(xiàn)三自由度的姿態(tài)測(cè)量。
[0010]其次、初始點(diǎn)的獲取:利用加速度計(jì)和陀螺儀芯片兩者的數(shù)據(jù)，進(jìn)行軌跡推算，以將軌跡周期提高到慣性測(cè)量單元的數(shù)據(jù)周期。眾所周知GPS有一定的誤差，如果GPS的誤差在15米范圍的話，遠(yuǎn)超過(guò)了車(chē)道的寬度，要想定位在車(chē)道上，確實(shí)是個(gè)難度?？梢岳霉潭℅PS基站的方式來(lái)校準(zhǔn)精度，但還有一個(gè)問(wèn)題，芯片提供的定位周期最快只有一秒。就是說(shuō)，這一秒范圍內(nèi)的軌跡是空白的，當(dāng)速度達(dá)到一定程度的時(shí)候，一秒的距離已經(jīng)很遠(yuǎn)，甚至已經(jīng)完成了一個(gè)快速小角度偏轉(zhuǎn)動(dòng)作。那么這樣的話精度將會(huì)大大降低。為了填補(bǔ)這一秒中的軌跡空白，于是我們利用加速度計(jì)和陀螺儀芯片，利用兩者數(shù)據(jù)推算軌跡，將軌跡周期提高到慣性測(cè)量單元的數(shù)據(jù)周期，即20毫秒。
[0011]第三、速度和移動(dòng)距離的計(jì)算:根據(jù)修正過(guò)的歐拉角，計(jì)算三軸的加速度的分量，從而計(jì)算出實(shí)際的加速度；在已知初速度，加速度和時(shí)間的情況下，通過(guò)對(duì)初速度積分和加速度二次積分，計(jì)算出移動(dòng)距離；慣性加速度矢量圖如圖2所示，得到了慣性加速度矢量的三個(gè)分量，根據(jù)計(jì)算得出的歐拉角即可計(jì)算出我們實(shí)際方向的加速度。
[0012]第四、計(jì)算位置點(diǎn):根據(jù)移動(dòng)距離和方向計(jì)算當(dāng)前的位置點(diǎn)的經(jīng)緯度，即在地球上已知一點(diǎn)，距離和方向，可以求出另一點(diǎn)的經(jīng)緯度。
[0013]第五:糾正位置:根據(jù)計(jì)算的位置點(diǎn)，在道路上進(jìn)行投影，進(jìn)行平滑處理，得到糾正后的位置點(diǎn)，如圖3所示。
【權(quán)利要求】
1.一種陀螺儀輔助定位方法，其特征在于包括以下步驟: 首先、姿態(tài)角的測(cè)量:對(duì)加速度計(jì)的測(cè)量值和磁傳感器的測(cè)量值分別經(jīng)過(guò)高斯牛頓迭代得到姿態(tài)誤差速率，陀螺儀直接通過(guò)四元數(shù)微分方程得到姿態(tài)四元數(shù)速率，兩個(gè)加起來(lái)積分得到姿態(tài)四元數(shù)； 其次、初始點(diǎn)的獲取:利用加速度計(jì)和陀螺儀芯片兩者的數(shù)據(jù)，進(jìn)行軌跡推算，以將軌跡周期提高到慣性測(cè)量單元的數(shù)據(jù)周期； 第三、速度和移動(dòng)距離的計(jì)算:根據(jù)修正過(guò)的歐拉角，計(jì)算三軸的加速度的分量，從而計(jì)算出實(shí)際的加速度；在已知初速度，加速度和時(shí)間的情況下，通過(guò)對(duì)初速度積分和加速度二次積分，計(jì)算出移動(dòng)距離； 第四、計(jì)算位置點(diǎn):根據(jù)移動(dòng)距離和方向計(jì)算當(dāng)前的位置點(diǎn)的經(jīng)緯度； 第五:糾正位置:根據(jù)計(jì)算的位置點(diǎn)，在道路上進(jìn)行投影，進(jìn)行平滑處理，得到糾正后的位置點(diǎn)。
【文檔編號(hào)】G01C21/16GK103644911SQ201310607804
【公開(kāi)日】2014年3月19日 申請(qǐng)日期:2013年11月27日 優(yōu)先權(quán)日:2013年11月27日
【發(fā)明者】閆智勇, 李建平 申請(qǐng)人:南京城際在線信息技術(shù)有限公司