本發(fā)明屬于室內(nèi)導航定位
技術(shù)領(lǐng)域：
，具體涉及一種室內(nèi)定位航向誤差校正方法。
背景技術(shù)：
：傳統(tǒng)的室外定位技術(shù)借助于全球定位系統(tǒng)(GlobalPositioningSystem,GPS)，利用衛(wèi)星和移動對象之間的直接通信，已可以完成精確的定位。隨著生活水平的提高和移動互聯(lián)網(wǎng)技術(shù)的發(fā)展，人們對于室內(nèi)導航定位的需求日益強烈。在大型的購物商場，建筑物內(nèi)部結(jié)構(gòu)復雜，種類繁多的商鋪分布在不同的角落，顧客短時間內(nèi)無法找到目的商品，此時如果提供室內(nèi)導航定位服務，為顧客提供商鋪的位置和商品信息，可以幫助顧客快速地找到目標商鋪。在多層建筑中發(fā)生火災時，迅速撤離并非易事，這時需要室內(nèi)導航定位服務指導處于危險的人群迅速、準確地撤離至安全地帶，防止發(fā)生重大的人員傷亡和財產(chǎn)損失。傳統(tǒng)的GPS信號由于受到建筑物等的遮擋，無法準確測距，在室內(nèi)環(huán)境下很難完成定位。當前出現(xiàn)了很多室內(nèi)導航定位技術(shù)，取得了不錯的效果。導航最重要的功能是提供載體準確的航向角信息，由于加速度的零偏誤差和陀螺儀的漂移等，單純根據(jù)慣性測量單元(IMU)輸出的航向角并不能滿足要求，由加速度和磁力計組成的組合測向設(shè)備可以提高航向的準確性和可靠性。但是磁力計在使用過程中會受到外界磁場的干擾，需要相應的校正和補償。準確實時的磁力計校正和補償技術(shù)是航向輸出的關(guān)鍵所在。利用加速度和磁力計的組合測向裝置通過測量地球磁場在載體三軸的分量大小確定航向，具有抗干擾、無輻射、體積小和成本低等優(yōu)點。但是磁力計會引入安裝誤差、零位誤差和標度因數(shù)誤差和外界磁質(zhì)干擾等誤差。前三種誤差不依賴于實際應用環(huán)境，一次補償即可永久使用，但是實際使用環(huán)境中的磁質(zhì)干擾根據(jù)強度和方向的不同，需要實時校正和補償。目前，出現(xiàn)了很多針對磁力計的校正和補償技術(shù)，一些方法綜合考慮了磁力計的誤差，對誤差進行建模，確定模型參數(shù)進而完成校正，但是在并不能滿足實時高精度的校正要求；另一些方法將環(huán)境的磁質(zhì)干擾建模為一個通用的誤差模型，沒有充分考慮不同環(huán)境之間的差異性。本發(fā)明正是在這樣的背景下，設(shè)計了一種基于非線性模型的室內(nèi)定位航向誤差校正方法，該方法能夠根據(jù)實際的磁質(zhì)干擾環(huán)境自適應地確定相應的誤差模型參數(shù)，實時準確地完成校正。本方法操作簡單，無需額外的輔助裝置，具有自適應程度高、實時性好，普適性強等優(yōu)點。技術(shù)實現(xiàn)要素：本發(fā)明提供了一種室內(nèi)定位航向誤差校正方法，該誤差校正方法能夠自適應地確定航向誤差校正模型，準確完成誤差校正與精度補償，解決了室內(nèi)定位和導航過程中航向校正的問題。綜上所述，為達到上述目的，本發(fā)明所采用的技術(shù)方案如下：一種室內(nèi)定位航向誤差校正方法，該校正方法采用航向測量裝置包括加速度計和磁力計，其特征在于，按照如下步驟實現(xiàn)：步驟一：選取校正方向，在校正環(huán)境中均勻選取n個校正方向，航向真實值記為步驟二：測量校正方向，利用加速度計和磁力計組合測向裝置測量所述步驟一中選取的校正方向，得到相應的航向測量值步驟三：計算航向誤差并建立航向誤差校正模型，對所述步驟二中的航向測量值和所述步驟一中的航向真實值作差，得到航向誤差并把所述航向誤差看做所述步驟二中航向測量值的非線性函數(shù)，建立參數(shù)未知的航向誤差校正模型；步驟四：根據(jù)預設(shè)閾值進行曲線擬合，確定所述步驟三中航向誤差校正模型的參數(shù)；步驟五：根據(jù)所述步驟四中航向誤差校正模型來校正實際的航向。進一步，所述步驟三中的非線性函數(shù)為：其中為所述航向誤差，為所述航向測量值，A,B,…K,M為所述航向誤差校正模型的參數(shù)，N是代表函數(shù)的最高次冪，且N≠0。進一步，所述步驟四中確定航向誤差校正模型參數(shù)的方法為：1)將所述公式(9)看成所述航向測量值的N次函數(shù)，寫成方程組形式為：其中代表第i個校正方向的誤差，代表第i個校正方向上的航向測量值，A,B,…K,M為方程參數(shù)；2)根據(jù)自適應擬合法計算N的值；3)利用最小二乘法求解所述公式(9)的函數(shù)參數(shù)A,B,…K,M。進一步，所述自適應擬合法，其特征在于，具體為：對于某次擬合的N值，N的初值為1，若則N即為所取，否則N＝N+1,繼續(xù)下一次擬合；其中為利用所述航向真實值求得的擬合函數(shù)值，為所述航向誤差。進一步，所述利用最小二乘法求解函數(shù)參數(shù)A,B,…K,M，其特征在于，具體為：根據(jù)得到的擬合值N，將公式(10)所述的方程組寫成矩陣形式為：根據(jù)最小二乘法計算參數(shù)A,B,…K,M為：進一步，所述步驟二中航向測量值的測量方法為：取導航坐標系為東北天坐標系(ENU)，載體坐標系為Oxbybzb，其中Oyb沿載體前向方向，Oxb軸垂直于前向朝右，Ozb軸與Oyb軸和Oxb軸構(gòu)成右手坐標系，利用載體坐標系下三軸加速度值(ax,ay,az)計算載體橫滾角roll和俯仰角pitch，如下：roll=arctan(ay/az)pitch=arctan(-axay2+az2)---(1)]]>根據(jù)所述公式(1)計算載體坐標系向?qū)Ш阶鴺讼缔D(zhuǎn)換的旋轉(zhuǎn)矩陣：Rbt=1000cosrsinr0-sinrcosrcosθ0-sinθ010sinθ0cosθ=cosθ0-sinθsinθ·sinrcosrcosθ·sinrsinθ·cosr-sinrcosθ·cosr---(2)]]>其中，其中θ＝pitch，r＝roll，均由式(1)計算而得；將載體任意姿態(tài)的三軸磁強轉(zhuǎn)換到導航坐標系下的磁場強度MxtMytMzt=Rbt·MxbMybMzb---(3)]]>根據(jù)導航坐標系下的磁場強度值計算航向角：進一步，所述步驟三中所述的航向誤差校正模型的建立方法為：磁力計所在環(huán)境的軟磁和硬磁干擾根據(jù)磁場的強度和方向各不相同，成為航向輸出的主要誤差來源。假設(shè)周圍無磁場干擾，磁力計輸出的x軸地磁分量和y軸磁強分量將構(gòu)成一個正圓；硬磁干擾產(chǎn)生于永久的磁質(zhì)材料或者被磁化后的金屬，它會使磁場強度輸出曲線的圓心發(fā)生偏移，對最終航向的影響是周期性且非線性的，誤差建模為：軟磁干擾產(chǎn)生于磁力計附近磁性材料之間的相互作用，它會使磁場強度輸出的曲線由正圓變成橢圓，對最終航向的影響也是周期性且非線性的，誤差建模為：因此磁力計受到的硬磁和軟磁干擾可建模為：根據(jù)fhm和fsm的周期性和非線性可知f也具有周期性和非線性。因此，進一步可將航向誤差建模為：其中表示航向誤差，表示航向測量值，A,B,…K,M為函數(shù)的參數(shù)，N是代表函數(shù)的最高次冪。由上述發(fā)明的方案可知，本發(fā)明的有益效果在于：1)本發(fā)明提出的方法成本低、精度高，普適性強，能夠顯著提高導航航向精度，進而提高導航定位系統(tǒng)的可靠性。2)本發(fā)明提出的方法具有較強的自適應性，可以根據(jù)校正環(huán)境實時確定航向誤差校正模型，準確完成誤差校正與精度補償。3)本發(fā)明的裝置構(gòu)成及方案設(shè)計簡單易行，只利用少許幾個航向真實值即可完成誤差模型的確定，因而方法的復雜度較低。4)本發(fā)明提出的方法與傳統(tǒng)方法相比，不是單純地對磁場強度進行校正，而是對最終的輸出航向誤差建模，充分考慮了航向誤差影響因素，因此校正結(jié)果更加可靠。附圖說明圖1為磁力計環(huán)境中硬磁干擾輸出曲線圖；圖2為磁力計環(huán)境中硬磁干擾對航向的周期性干擾圖；圖3為磁力計環(huán)境中軟磁干擾輸出曲線圖；圖4為磁力計環(huán)境中軟磁干擾對航向的周期性干擾圖；圖5為航向校正流程圖；圖6為誤差模型曲線擬合結(jié)果圖；圖7為校正前后誤差對比圖。具體實施方式下面結(jié)合附圖和具體的實施例對本發(fā)明做進一步詳細的說明，以便更好地理解本發(fā)明，但本發(fā)明的保護范圍不限于此。實施例選用的測量裝置為MPU6050加速度計和HMC5883磁力計慣性測量模塊，兩者具有相同的或者平行的坐標軸定義。MPU6050具有±16g的測量量程，穩(wěn)定性為0.01g，分辨率高達6.1×10-5g，HMC5883磁力計的測量范圍為[-8,8]Gauss，可精確到1mgauss，利用杜邦線將模塊與開發(fā)板連接通過MiniUSB線與上位機通信存儲數(shù)據(jù)。如圖5所示，本實施例的實施環(huán)境為一典型的室內(nèi)環(huán)境，由于環(huán)境中存在大量的電磁材料而具有明顯的磁質(zhì)干擾，一種室內(nèi)定位航向誤差校正方法，該校正方法具體的實施步驟為：步驟一：在0°～360°內(nèi)每隔30°選定一個校正方向，航向真實值記為步驟二：利用所述測量裝置分別測量所述步驟一中選取的校正方向的加速度和磁場強度數(shù)據(jù)，得到相應的航向測量值根據(jù)加速度數(shù)據(jù)計算載體的橫滾角和俯仰角，進而計算載體坐標系向?qū)Ш阶鴺讼缔D(zhuǎn)換的旋轉(zhuǎn)矩陣。由所述的旋轉(zhuǎn)矩陣將載體坐標系下的磁場強度轉(zhuǎn)換到導航坐標系下，根據(jù)公式(4)計算航向角。取導航坐標系為東北天坐標系(ENU)，載體坐標系為Oxbybzb，其中Oyb沿載體前向方向，Oxb軸垂直于前向朝右，Ozb軸與Oyb軸和Oxb軸構(gòu)成右手坐標系，利用載體坐標系下三軸加速度值(ax,ay,az)計算載體橫滾角roll和俯仰角pitch，如下：roll=arctan(ay/az)pitch=arctan(-axay2+az2)---(1)]]>根據(jù)公式(1)計算載體坐標系向?qū)Ш阶鴺讼缔D(zhuǎn)換的旋轉(zhuǎn)矩陣：Rbt=1000cosrsinr0-sinrcosrcosθ0-sinθ010sinθ0cosθ=cosθ0-sinθsinθ·sinrcosrcosθ·sinrsinθ·cosr-sinrcosθ·cosr---(2)]]>其中，其中θ＝pitch，r＝roll，均由式(1)計算而得。將載體任意姿態(tài)的三軸磁強轉(zhuǎn)換到導航坐標系下的磁場強度MxtMytMzt=Rbt·MxbMybMzb---(3)]]>根據(jù)導航坐標系下的磁場強度值計算航向角：步驟三：計算航向誤差并建立航向誤差校正模型，對所述步驟二中得到的航向測量值和所述步驟一中航向真實值作差，得到航向誤差并把所述航向誤差看做所述步驟二中航向測量值的非線性函數(shù)，建立參數(shù)未知的航向誤差校正模型；航向誤差校正模型的建立方法為：磁力計所在環(huán)境的軟磁和硬磁干擾根據(jù)磁場的強度和方向各不相同，成為航向輸出的主要誤差來源。假設(shè)周圍無磁場干擾，磁力計輸出的x軸地磁分量和y軸磁強分量將構(gòu)成一個正圓。硬磁干擾產(chǎn)生于永久的磁質(zhì)材料或者被磁化后的金屬，這種干擾會使磁場強度與真實值產(chǎn)生偏差，如圖1所示，以X方向磁場分量為橫軸，以Y方向磁場分量為縱軸，圖中的圓的圓心發(fā)生了偏移。而硬磁干擾對航向的影響呈現(xiàn)非線性關(guān)系，如圖2所示，以航向為橫軸，以航向誤差為縱軸，磁力計環(huán)境中硬磁干擾對最終航向的影響是周期性且非線性的，將其建模為：軟磁干擾產(chǎn)生于磁力計附近磁性材料之間的相互作用，它會使磁場強度輸出的曲線由正圓變成橢圓，如圖3所示，以X方向磁場分量為橫軸，以Y方向磁場分量為縱軸，本來應該為圓形的磁強分布變成了橢圓形；圖4顯示了軟磁干擾對航向誤差的影響曲線，以航向為橫軸，以航向誤差為縱軸，軟磁干擾對最終航向的影響也是周期性且非線性的，誤差建模為：因此磁力計受到的硬磁和軟磁干擾可建模為：根據(jù)fhm和fsm的周期性和非線性可知f也具有周期性和非線性。因此，進一步可將航向誤差建模為：其中表示航向誤差，表示航向測量值，A,B,…K,M為函數(shù)的參數(shù)，N是代表函數(shù)的最高次冪。綜上所述，所述非線性函數(shù)為：其中為所述航向誤差，為所述航向測量值，A,B,…K,M為所述航向誤差校正模型的參數(shù)，N是代表函數(shù)的最高次冪，且N≠0。步驟四：根據(jù)預設(shè)門限閾值進行曲線擬合，確定所述步驟三中航向誤差校正模型的參數(shù)；1)確定航向誤差校正模型參數(shù)的方法為：將所述公式(9)看成所述航向測量值的N次函數(shù)，寫成方程組形式為：其中Δφi,i＝[1:n]代表第i個校正方向的誤差，代表第i個校正方向上的航向測量值，A,B,…K,M為方程參數(shù)。2)根據(jù)自適應擬合法計算N的值；對于某次擬合的N值，N的初值為1，若則N即為所取，否則N＝N+1,繼續(xù)下一次擬合。其中為利用所述航向真實值求得的擬合函數(shù)值，為所述航向誤差。3)利用最小二乘法求解所述公式(9)的函數(shù)參數(shù)A,B,…K,M具體為：根據(jù)得到的擬合值N，將所述公式(10)所述的方程組寫成矩陣形式為：根據(jù)最小二乘法計算參數(shù)A,B,…K,M為：如圖6所示，以采樣航向值為橫軸，以航向誤差為縱軸作出的實際測量曲線和擬合曲線，擬合得到N＝4，相應的誤差模型系數(shù)為：Coef＝[-0.0068,0.0944,-0.3545,0.2051,0.2844]。步驟五：根據(jù)所述步驟四中航向誤差校正模型來校正實際的航向。步驟六：將所述測量裝置旋轉(zhuǎn)一周，每隔15°采集一次數(shù)據(jù)，用于檢驗方法的正確性；如表1所示，表1顯示了本發(fā)明所述方法校正前后的誤差對比狀況。表1航向校正前后誤差對比圖7為利用所述的方法對航向進行校正前后的誤差對比圖，以航向采樣值為橫軸，以航向誤差為縱軸，作出的校正前航向誤差曲線和校正后航向誤差曲線。表2為對航向誤差校正前后的平均誤差和誤差標準差進行統(tǒng)計的數(shù)據(jù)。表2航向校正前后主要精度指標對比從表2可以看出，利用發(fā)明所述的方法對航向校正前，平均誤差為12.9°，標準差為15.4°，利用所述方法校正后的平均誤差為3.2°，標準差為3.4°。較大地提高了航向精度。要說明的是，上述實施例是對本發(fā)明技術(shù)方案的說明而非限制，所屬
技術(shù)領(lǐng)域：
普通技術(shù)人員的等同替換或者根據(jù)現(xiàn)有技術(shù)而做的其它修改，只要沒超出本發(fā)明技術(shù)方案的思路和范圍，均應包含在本發(fā)明所要求的權(quán)利范圍之內(nèi)。當前第1頁1 2 3