一種室內慣性導航定位的航向漂移誤差消除方法
【技術領域】
[0001] 本發(fā)明設及室內導航領域，特別是一種室內慣性導航定位的航向漂移誤差消除方 法。
【背景技術】
[0002] 隨著社會的發(fā)展，人們對室內位置服務的需求越來越迫切，如可W應用于手機購 物、移動電子商務、個性化廣告/優(yōu)惠信息等。用戶會希望能夠直接獲取商店或者所需產品 的位置，而不用耗費大量時間苦苦尋找。由于GI^S等文星導航定位方式無法在室內環(huán)境中 正常工作，所W如何解決室內定位問題成為近年來人們研究的重點。
[0003] 目前，已有的室內定位系統(tǒng)的解決方案主要有：WiFi定位、超聲定位、射頻定位和 慣性導航定位等。慣性導航定位系統(tǒng)具有低成本，自主定位，可W在任何環(huán)境中工作的特 點，越來越受到人們的青睞，成為各大研究機構研究的熱點。
[0004] 室內行人慣性導航定位系統(tǒng)的原理如下；首先，由S軸巧螺儀測量的S軸角速度， 通過四元數(shù)微分方程計算實時姿態(tài)四元數(shù)，由姿態(tài)四元數(shù)可W得到航向角和姿態(tài)角。然后， 通過四元數(shù)姿態(tài)矩陣，將載體坐標系的=軸加速度變換到導航坐標系（東-北-天）。最后， 對導航坐標系的加速度進行積分運算可W得到每一步的步長，每一步的步長和航向進行矢 量疊加就可W得到行人的運動軌跡，從而實現(xiàn)導航定位的功能。由此可見，影響室內行人慣 性導航系統(tǒng)定位精度的主要原因是積分漂移誤差和航向漂移誤差，航向漂移是指由于靜止 時巧螺儀不工作，導致由姿態(tài)四元數(shù)解算出來的航向角W-定的斜率變化，嚴重影響室內 行人慣性導航定位系統(tǒng)的精度，消除積分漂移誤差和航向漂移誤差成為提高定位精度的關 鍵。

【發(fā)明內容】

[0005] 本發(fā)明的主要目的在于提出一種室內慣性導航定位的航向漂移誤差消除方法，可 W有效的解決由于靜止時巧螺儀不工作導致的航向漂移誤差，從而提高室內行人慣性導航 定位系統(tǒng)的精度。
[0006] 本發(fā)明采用如下技術方案：
[0007] -種室內慣性導航定位的航向漂移誤差消除方法，基于慣性測量單元實現(xiàn)，該慣 性測量單元包括有立軸加速度計、立軸磁力計和立軸巧螺儀，其特征在于；預先通過立軸 加速度計和=軸磁力計來計算姿態(tài)四元數(shù)初值；在室內慣性導航定位過程中，先通過= 軸巧螺儀讀取角速度來判斷慣性測量單元是否處于運動狀態(tài)，若是，則結 合角速度并通過四元數(shù)微分方程;^ @ 0計算實時的姿態(tài)四元數(shù) Q(q。，屯，屯，Q3)，若否，則保持當前姿態(tài)四元數(shù)不變；最后，通過實時的姿態(tài)四元數(shù)求解慣性 測量單元的航向角和姿態(tài)角。
[000引優(yōu)選的，所述的姿態(tài)四元初值通過如下步驟計算得到：首先，通過=軸加速度計 確定初始姿態(tài)角，根據該初始姿態(tài)角計算坐標變換矩陣；其次，通過=軸磁力計的磁場強度 結合坐標變換矩陣得到與當?shù)厮矫嫫叫械淖鴺讼档拇艌鰪姸?，根據該坐標系的X軸和Y 軸的磁場強度計算初始航向角；最后，通過歐拉角和四元數(shù)之間的轉換公式，得到初始姿態(tài) 四元數(shù)。
[0009] 優(yōu)選的，通過S軸加速度計得到的加速度（a。a,，a,)確定初始姿態(tài)角；
【主權項】
1. 一種室內慣性導航定位的航向漂移誤差消除方法，基于慣性測量單元實現(xiàn)，該慣 性測量單元包括有三軸加速度計、三軸磁力計和三軸陀螺儀，其特征在于：預先通過三軸 加速度計和三軸磁力計來計算姿態(tài)四元數(shù)初值；在室內慣性導航定位過程中，先通過三軸 陀螺儀讀取角速度（《x，《y，《z)來判斷慣性測量單元是否處于運動狀態(tài)，若是，則結合 角速度《 (?x，《y，《z)并通過四元數(shù)微分方程
計算實時的姿態(tài)四元數(shù) Q(qtl，qi，q2,q3)，若否，則保持當前姿態(tài)四元數(shù)不變；最后，通過實時的姿態(tài)四元數(shù)求解慣性 測量單元的航向角和姿態(tài)角。
2. 如權利要求1所述的一種室內慣性導航定位的航向漂移誤差消除方法，其特征在 于：所述的姿態(tài)四元初值通過如下步驟計算得到：首先，通過三軸加速度計確定初始姿態(tài) 角，根據該初始姿態(tài)角計算坐標變換矩陣；其次，通過三軸磁力計的磁場強度結合坐標變換 矩陣得到與當?shù)厮矫嫫叫械淖鴺讼档拇艌鰪姸?，根據該坐標系的X軸和Y軸的磁場強度 計算初始航向角；最后，通過歐拉角和四元數(shù)之間的轉換公式，得到初始姿態(tài)四元數(shù)。
3. 如權利要求2所述的一種室內慣性導航定位的航向漂移誤差消除方法， 其特征在于：通過三軸加速度計得到的加速度（ax,ay,az)確定初始姿態(tài)角：
'其中，9為俯仰角，y為翻滾角，g為重力加速度。
4. 如權利要求3所述的一種室內慣性導航定位的航向漂移誤差消除方法，其特征在 于：通過初始姿態(tài)角計算坐標變換矩陣具體是指： 記三軸磁力計的初始位置坐標系為b，與當?shù)厮矫嫫叫械淖鴺讼禐閚，中間坐標系為 1，坐標變換矩陣為G ?其中：
5. 如權利要求4所述的一種室內慣性導航定位的航向漂移誤差消除方法，其特征在 于：記三軸磁力計磁場強度為(<_，i<，m〗），通過坐標變換矩陣得到與當?shù)厮矫嫫叫凶鴺?系n的磁場強度：(<，<，m〗)7 =q(m〗，mX,，由坐標系n的X軸和Y軸的磁場強度可 以計算此時的初始航向角：
6. 如權利要求5所述的一種室內慣性導航定位的航向漂移誤差消除方法，其特征在 于：通過歐拉角和四元數(shù)之間的轉換公式，得到初始姿態(tài)四元數(shù)Q(qtl，qi，q2,q3):
7.如權利要求1所述的一種室內慣性導航定位的航向漂移誤差消除方法，其特征在 于：當所述的保持當前姿態(tài)四元數(shù)不變，具體是指：當判定慣性測量單元處于靜止狀態(tài)，則 設定三軸陀螺儀輸出的角速度《x= ? y= ? z= 0,使得四元數(shù)的增量為零，從而實現(xiàn)保持 當前姿態(tài)四元數(shù)不變。
【專利摘要】一種室內慣性導航定位的航向漂移誤差消除方法，基于慣性測量單元實現(xiàn)，該慣性測量單元包括有三軸加速度計、三軸磁力計和三軸陀螺儀，預先通過三軸加速度計和三軸磁力計來計算姿態(tài)四元數(shù)初值；在室內慣性導航定位過程中，先通過三軸陀螺儀讀取角速度(ωx,ωy,ωz)來判斷慣性測量單元是否處于運動狀態(tài)，若是，則結合角速度ω(ωx,ωy,ωz)并通過四元數(shù)微分方程計算實時的姿態(tài)四元數(shù)Q(q0,q1,q2,q3)，若否，則保持當前姿態(tài)四元數(shù)不變；最后，通過實時的姿態(tài)四元數(shù)求解慣性測量單元的航向角和姿態(tài)角。本發(fā)明提出的航向漂移誤差的消除方法，可以有效的解決由于靜止時陀螺儀不工作導致的航向漂移誤差，從而提高室內行人慣性導航定位系統(tǒng)的精度。
【IPC分類】G01C25-00, G01C21-16
【公開號】CN104567931
【申請?zhí)枴緾N201510017603
【發(fā)明人】莫冰, 朱金林, 曾顯彬, 楊逸純, 毛文輝, 蔡鈞
【申請人】華僑大學, 福州曲直電子科技有限公司
【公開日】2015年4月29日
【申請日】2015年1月14日