【基于慣性導航的無人車導航技術(shù)探究8200字（論文）】

基于慣性導航的無人車導航技術(shù)研究摘要隨著科技的發(fā)展，無人系統(tǒng)進入到了越來越多的領(lǐng)域當中，如自動駕駛汽車、無人快遞投放小車、無人倉庫管理系統(tǒng)等。在無人系統(tǒng)中，實現(xiàn)精確的定位和導航至關(guān)重要。在室外情況下，全球衛(wèi)星導航系統(tǒng)(GlobalNavigationSatelliteSystem,GNSS)的日益成熟可以提供精確的導航服務(wù)，而在室內(nèi)情況下，由于很難獲取衛(wèi)星信號，難以發(fā)揮作用。隨著人們對于室內(nèi)定位導航的需求加大，出現(xiàn)了許多室內(nèi)定位方案。但是單一的定位技術(shù)難以滿足實際應(yīng)用中的精度要求，固本文以無人小車為研究對象，研究了基于擴展卡爾曼濾波的UWB/IMU的組合定位算法；為了解決物體在運動過程中量測噪聲協(xié)方差矩陣出現(xiàn)突變，影響擴展卡爾曼濾波算法定位估計的問題，本文提出了基于簡化Sage-Husa自適應(yīng)卡爾曼濾波的UWB/IMU組合定位算法。最后對組合定位算法進行了仿真，并將本文中所建立的UWB/IMU組合定位系統(tǒng)在搭建的試驗車載平臺上進行了實物試驗，實物試驗的結(jié)果表明，本文提出的基于UWB/IMU的室內(nèi)無人車導航算法有效的提高了定位精度。關(guān)鍵詞：慣性導航；無人車導航；UWB；卡爾曼濾波；組合定位目錄TOC\o"1-3"\h\u165741引言 1306921.1研究背景 168621.2研究意義 1153412慣性導航行為算法 1210372.1坐標系及其轉(zhuǎn)換 174332.1.1常用坐標系 1256792.1.2坐標系轉(zhuǎn)換 3258222.2捷聯(lián)慣導系統(tǒng)速度和位置更新 3138892.3定位結(jié)果評價方法 416202.3.1圓概率誤差 4207532.3.2誤差累積分布函數(shù) 4268582.3.3最大定位誤差 4323522.3.4均方根定位誤差 559953組合定位算法 5119593.1基于卡爾曼濾波的定位算法 5136953.2基于擴展卡爾曼濾波的組合導航 780203.3基于自適應(yīng)卡爾曼濾波的組合定位 10314174定位導航試驗 1090994.1車載試驗平臺 1199924.2實物試驗 13219914.2.1靜態(tài)定位試驗 13173344.2.2基于卡爾曼濾波的UWB定位系統(tǒng) 14224554.2.3組合定位 16238975結(jié)論 1914768參考文獻 201引言1.1研究背景隨著科技的發(fā)展，無人系統(tǒng)進入到了越來越多的鄰域當中，如自動駕駛汽車、無人快遞投放小車、無人倉庫管理系統(tǒng)等。在無人系統(tǒng)中，實現(xiàn)精確的定位和導航至關(guān)重要?；趹T性導航的室內(nèi)定位技術(shù)不需要在使用場景中布置其他設(shè)施，是自主導航的一種，利用陀螺儀、加速度計等慣性元器件對載體的運動狀態(tài)進行測量。具有完全自主、高隱蔽、強抗擾和信息連續(xù)等特點。上述的定位技術(shù)在實際中都有應(yīng)用，但這些技術(shù)有著自身固有的局限性，如一些無線定位技術(shù)需要在定位環(huán)境中預先安裝定位設(shè)施，慣性導航定位系統(tǒng)對慣性器件的精度要求比較高，所以這些定位系統(tǒng)只在特定的環(huán)境中有相對較好的效果。綜上所述，需要研究一種精度高、應(yīng)用廣的室內(nèi)定位系統(tǒng)。1.2研究意義在短時間內(nèi)和短時間情況下，慣性導航可以做到高精度的自主導航，然而在運動過程中隨著時間的增加，慣性導航會累積導航誤差使得導航結(jié)果不準確。將慣性導航系統(tǒng)和其他定位方式組合在一起，消除累積誤差，是提高導航精度的一個好辦法。UWB具有傳輸速率快、傳輸功耗低、定位精度高等優(yōu)點。本課題將使用UWB定位技術(shù)和慣性導航（IMU）技術(shù)，將兩者融合到一起，發(fā)揮兩者的優(yōu)勢，彌補兩者的缺點，從而實現(xiàn)更加精確地室內(nèi)定位導航。2慣性導航行為算法2.1坐標系及其轉(zhuǎn)換2.1.1常用坐標系（1）地球坐標系定義地球坐標系中坐標原點為地球的中心，x、y、z三軸滿足右手定律，x軸沿地球赤道平面和本初子午面的交線，z軸指向地球北極。地球坐標系如圖2-1所示。圖2-1地球坐標系（2）地理坐標系如圖2-2所示，地理坐標系的原點為載體所在位置，x軸方向指向該地地理位置的東方向，y軸方向指向該地地理位置的北方向，z軸方向是垂直于地理位置水平面的向上方向，x、y、z三軸滿足右手定律（z=x*y），構(gòu)成常用的東北天坐標系（East-North-Up,ENU）。圖2-2地理坐標系（3）載體坐標系定義載體坐標系的原點在載體重心，x軸指向載體的正右方，y軸指向載體的正前方，z軸指向載體的正上方。三軸滿足右手定律，載體坐標系如圖所示。圖2-3載體坐標系（4）導航坐標系本篇文章中選取動捕系統(tǒng)坐標系作為導航坐標系。2.1.2坐標系轉(zhuǎn)換導航系統(tǒng)中需要坐標系之間的轉(zhuǎn)換，坐標系的轉(zhuǎn)換可以通過平移和旋轉(zhuǎn)實現(xiàn)，將兩個坐標系的原點通過平移重合在一起，然后通過有限次的單軸旋轉(zhuǎn)使坐標系重合從而得到坐標系的轉(zhuǎn)換。捷聯(lián)慣導系統(tǒng)中，加速度計的量測值為是在載體坐標系下的值，實際應(yīng)用中，需要轉(zhuǎn)換到導航坐標系中才有意義。因此，本文要完成載體坐標系到導航坐標系的轉(zhuǎn)換，因為選用的導航坐標系是地理坐標系，兩個坐標系的原點是重合的，所以只要需要通過旋轉(zhuǎn)就可以實現(xiàn)坐標系的轉(zhuǎn)換。坐標轉(zhuǎn)換可以使用四元數(shù)，歐拉角等形式來表示。雖然歐拉角存在萬向鎖的問題，但是在本文的試驗環(huán)境中，不存在這種問題，而且相較于四元數(shù)來說，歐拉角形式更簡單，理解更方便，所以本文使用歐拉角來表示坐標系之間的轉(zhuǎn)換。2.2捷聯(lián)慣導系統(tǒng)速度和位置更新在載體坐標系下加速度計的量測值為ab如下式所示：(2-1)通過旋轉(zhuǎn)矩陣將載體坐標系下的加速度值ba轉(zhuǎn)換到導航坐標系下的加速度值an1中：(2-2)再將導航坐標系下的加速度值an1減去重力矢量，得到加速度an。(2-3)2.3定位結(jié)果評價方法2.3.1圓概率誤差圓概率誤差(CEP)在之前的應(yīng)用中是對炮彈或者導彈的命中精度做評判。在二維平面中，圓概率誤差的定義為以誤差的均值為中心，包含50%的定位估計值的圓域半徑，是對定位準確性的一種簡單的表示。設(shè)定位的橫向誤差為x，定位的縱向誤差為與，x和y服從正態(tài)分布，當x和y相互獨立且標準差不同時，圓概率誤差可以表示為：(2-4)或者圓誤差概率可以表示為：(2-5)σx為橫向誤差標準差，σy為縱向誤差標準差。公式一般為軍方使用的計算公式。如果兩個標準差之間的差值比較大時，如σ1/σ2∈[0.2,1]時，公式2的近似程度比較高。2.3.2誤差累積分布函數(shù)誤差累計分布函數(shù)(CDF)是指誤差值X小于或等于x的概率，即：(2-6)在實際應(yīng)用中，需要大量試驗來統(tǒng)計處定位誤差，再利用分布函數(shù)對系統(tǒng)的定位精度進行評價。2.3.3最大定位誤差最大定位誤差(MPE)是定位誤差中的最大值，下式所示：(2-7)最大定位誤差可以評價系統(tǒng)的魯棒性。2.3.4均方根定位誤差均方根誤差(RMSE)可以對系統(tǒng)進行準確性評估，均方根誤差是指定位目標的位置真值和位置估計值之間的均方根計算值。計算公式為：(2-8)X和y表示為定位目標的定位真值，x和y表示為定位系統(tǒng)的位置估計值。均方根誤差是對定位精度一種簡單且精確的描述。3組合定位算法3.1基于卡爾曼濾波的定位算法濾波是在混合在一起的信號中濾除無用信號得到有用信號，但確切地說，卡爾曼濾波應(yīng)稱作為最優(yōu)估計理論，此處所說的濾波與常規(guī)的濾波具有完全不同的概念和含義?？柭鼮V波器有如下特點：（1）卡爾曼濾波是在時域上設(shè)計的，并且在多維情況下也同樣使用。（2）要被處理的信號無有用或者無用之分，卡爾曼濾波是為了估計要被處理的信號。（3）在計算過程中，系統(tǒng)噪聲和過程噪聲并不是要被濾除的對象，相反它們的統(tǒng)計特征正是卡爾曼濾波估計中所需的信息?？柭鼮V波有連續(xù)型和離散型兩類算法，離散型算法可直接在數(shù)字計算機上實現(xiàn)。在實際的應(yīng)用中，離散型卡爾曼濾波應(yīng)用更加廣泛，所以在這里，我們建立離散系統(tǒng)的模型。設(shè)系統(tǒng)的狀態(tài)向量為Xk，在tk時刻受系統(tǒng)的噪聲序列Wk?1驅(qū)動，建立離散系統(tǒng)模型為1-1所示(3-1)在模型中，第一個式子為系統(tǒng)的狀態(tài)方程，第二個式子為系統(tǒng)的量測方程。Φ表示為系統(tǒng)從tk-1時刻到tk時刻的狀態(tài)轉(zhuǎn)移矩陣；Γk-1表示為系統(tǒng)的噪聲驅(qū)動矩陣；Wk-1表示為在tk-1時刻系統(tǒng)的噪聲矩陣；H表示為系統(tǒng)的量測矩陣；Vk表示為在tk時刻系統(tǒng)的量測噪聲。在上式中，Wk?1和Vk還滿足下面的關(guān)系：(3-2)(3-3)其中Q為系統(tǒng)噪聲的協(xié)方差矩陣，R為量測噪聲的協(xié)方差矩陣。由于Wk-1和Vk相互獨立，因此還有。對于上面的系統(tǒng)狀態(tài)方程（1.1）來說，求取狀態(tài)向量Xk的估計值可通過以下式子求得：狀態(tài)一步預測：(3-4)協(xié)方差矩陣一步預測：(3-5)計算卡爾曼增益：(3-6)狀態(tài)估計：(3-7)協(xié)方差矩陣預測：(3-8)對于上面的計算流程可以用下面的算法流程圖表示：圖3-1卡爾曼濾波算法流程圖從圖中可以看出來，計算流程分為兩個部分，濾波計算回路部分和增益計算回路部分，其中濾波回路這一部分需要依賴于增益計算回路部分來運行。在一個計算周期中，卡爾曼濾波有兩個信息更新：時間更新和量測更新。上面的五個式子中，狀態(tài)的一步估計為根據(jù)上一次的計算周期所得到的估計值來得到這一次計算周期的先驗估計值。而系統(tǒng)噪聲協(xié)方差矩陣Q和量測噪聲協(xié)方差矩陣R影響著卡爾曼增益kK的大小，卡爾曼增益kK的大小在狀態(tài)估計方程中決定了狀態(tài)估計值更加接近于先驗估計值還是量測值，也就是說，估計值更偏向于系統(tǒng)方程的推算還是量測的結(jié)果。3.2基于擴展卡爾曼濾波的組合導航上一節(jié)中所討論的卡爾曼濾波問題是假設(shè)系統(tǒng)方程的數(shù)學模型為線性模型，但是在實際的工程應(yīng)用場景中，絕大部分遇到的系統(tǒng)都存在不同程度的非線性情況。如系統(tǒng)方程為非線性，或量測方程為非線性，或兩者都為非線性。在本課題中，基于測距的UWB定位系統(tǒng)就是非線性系統(tǒng)，因此有必要研究非線性卡爾曼濾波系統(tǒng)。在非線性卡爾曼濾波中，擴展卡爾曼濾波是一種簡單有效的估計方法。擴展卡爾曼濾波以線性卡爾曼濾波為基礎(chǔ)，其核心思想就是通過泰勒展開的方式將非線性方程線性化，從而將非線性系統(tǒng)在估計點處轉(zhuǎn)化為近似線性系統(tǒng)。非線性系統(tǒng)可通過以下方程表示：(3-8)在方程中，第一個式子為系統(tǒng)的狀態(tài)方程，第二個式子為系統(tǒng)的量測方程。f(?)為系統(tǒng)狀態(tài)方程的非線性函數(shù)；Γk?1表示為系統(tǒng)的噪聲驅(qū)動矩陣；Wk-1表示為在tk-1時刻系統(tǒng)的過程噪聲矩陣；h(?)為量測方程中的非線性函數(shù)，Vk是量測噪聲矩陣。設(shè)過程噪聲W和觀測噪聲V是服從均值為零，方差為Q和R的高斯白噪聲且互不相關(guān)。將上式中的系統(tǒng)狀態(tài)方程的非線性函數(shù)f(?)在估計值處一階泰勒展開，如下式所示：(3-9)(3-10)(3-11)則得到系統(tǒng)的狀態(tài)方程如下式所示：(3-12)令(3-13)(3-14)可以得到系統(tǒng)的量測方程為：(3-15)根據(jù)線性卡爾曼濾波的基本原理推導出擴展卡爾曼濾波的具體算法如下：預測狀態(tài)：(3-16)預測狀態(tài)協(xié)方差狀態(tài)：(3-17)計算卡爾曼增益：(3-18)更新狀態(tài)：(3-19)更新協(xié)方差矩陣：(3-20)在上述的計算過程中可以看出來，與線性卡爾曼濾波不同的是，擴展卡爾曼濾波的中的狀態(tài)轉(zhuǎn)移矩陣F和觀測矩陣H是f(?)和h(?)的雅克比矩陣。設(shè)系統(tǒng)的狀態(tài)變量為，雅克比矩陣的計算方式如下所示：(3-21)(3-22)3.3基于自適應(yīng)卡爾曼濾波的組合定位卡爾曼濾波作為一種最優(yōu)估計理論，應(yīng)用很廣泛，但是隨著應(yīng)用領(lǐng)域的擴大，很多場景不能滿足卡爾曼濾波的基本假設(shè)。對于某些場景來說，通常來說，難以建立精確的狀態(tài)模型和得到相應(yīng)的觀測噪聲特性，還有存在一些未知的干擾噪聲使得估計的精度降低，甚至會導致濾波發(fā)散。另外，當系統(tǒng)所處環(huán)境發(fā)生變化，傳感器的量測結(jié)果會存在野值，觀測量會發(fā)生突變，這樣也會嚴重影響卡爾曼濾波的估計結(jié)果。從上面的算法流程圖中可以看出來，Q矩陣和R矩陣會影響到卡爾曼增益K的大小。如果Q矩陣小于實際的噪聲分布或者是R矩陣大于觀測噪聲的分布，會導致卡爾曼增益K過小，從而導致估計值偏小；相反的話，如果Q矩陣大于實際的噪聲分布或者是R矩陣小于觀測噪聲的分布，會導致卡爾曼增益K過大，從而導致濾波發(fā)散。因此Q矩陣和R矩陣的選取很重要。在傳統(tǒng)卡爾曼濾波中，Q值和R值由經(jīng)驗值所確定。在計算過程中保持不變，會出現(xiàn)下面的問題。當測量結(jié)果Z(k)有異常值時，如果噪聲協(xié)方差矩R沒有進行調(diào)整的話，測量結(jié)果會對估計值有影響，使得估計結(jié)果不好甚至使卡爾曼濾波發(fā)散。當在過程中，系統(tǒng)的數(shù)學模型發(fā)生變化或者系統(tǒng)噪聲發(fā)生變化，系統(tǒng)噪聲協(xié)方差矩陣Q如果不進行調(diào)整的話，對系統(tǒng)的狀態(tài)描述不準確，一步預測值同樣也會對估計結(jié)果產(chǎn)生影響。因此，自適應(yīng)的調(diào)整噪聲協(xié)方差矩陣R和系統(tǒng)協(xié)方差矩陣Q，可以減少異常值對估計結(jié)果的影響。對于傳統(tǒng)卡爾曼濾波的不足，出現(xiàn)了各種改進的濾波算法，常見的濾波算法有漸消記憶法，限定記憶法，自適應(yīng)卡爾曼濾波，抗野值濾波等。4定位導航試驗4.1車載試驗平臺下圖為車載試驗平臺的具體搭建，圖中紅色方框為慣性傳感器，綠色方框中的為UWB模塊，黃色三角形框中為動作捕捉系統(tǒng)所要識別的對象。慣性傳感器和UWB模塊的信息通過USB傳輸?shù)纳衔粰C，動作捕捉系統(tǒng)的數(shù)據(jù)通過WiFi傳輸?shù)缴衔粰C。圖5-1車載平臺圖4-1車載平臺（1）車載平臺本次試驗所選用的車載試驗平臺為松靈公司出品的全能型UGVSCOUT2.0，SCOUT2.0整體上采用了模塊化和智能化的設(shè)計理念，在動力模塊上采用充氣膠輪與獨立懸掛的復合設(shè)計，使用直流無刷伺服電機，采用差速轉(zhuǎn)向方法。SCOUT2.0最大載重50kg，滿足試驗的需求。圖4-2SCOUT2.0試驗車輛（2）慣性傳感器模塊本次試驗，使用的慣性傳感器模塊是Xsens公司生產(chǎn)的MTi-G-710。MTi-G-710位置姿態(tài)傳感器是MTi-100系列產(chǎn)品中一款基于衛(wèi)星導航與慣性導航相結(jié)合的導航與穩(wěn)定控制應(yīng)用的解決方案。是具有一體式結(jié)構(gòu)的傳感器系統(tǒng)，可輸出高頻率的位置和姿態(tài)數(shù)據(jù)。下表為具體參數(shù)。表4-1MTi-G-710具體參數(shù)（3）UWB模塊本次試驗所使用的UWB模塊為Nooploop公司生產(chǎn)的LinkTrack-S型號的UWB模塊，該UWB模塊有以下幾個優(yōu)點。1）有局部定位、分布式測距、數(shù)傳三種模式；2）所有節(jié)點自動無線組網(wǎng)，無需拉線；3）定位、導航、授時、通信（PNTC）一體化；4）無需服務(wù)器即可運行，模塊實時解算；5）同一硬件可設(shè)置為標簽、基站、等角色；6）高容量與高刷新頻率：40標簽/8基站/1控制臺；7）測距、定位刷新頻率高達200Hz；8）1維、2維典型定位精度10cm，典型第3維定位精度30cm；9）獨立高速低延遲數(shù)傳模式，帶寬3Mbps；10）最遠通信距離80米；11）支持UART、USB通信；12）從3.5GHz到6.5GHz一共6個射頻頻段；13）發(fā)射增益可調(diào)范圍為0~33.5dB。4.2實物試驗4.2.1靜態(tài)定位試驗在靜態(tài)定位試驗中，測試場地如圖所示布置。在四基站條件下，四個基站布置在一個矩形場地的四個頂點上。在三基站條件下，三個基站布置在一個等腰三角形的三個頂點上。在試驗中，試驗小車行駛每隔一小段距離就停止并采集數(shù)據(jù)。UWB的數(shù)據(jù)更新頻率為50Hz，動作捕捉系統(tǒng)的采樣頻率為100Hz，在每一個數(shù)據(jù)采集點，采集的時長為10秒以上。通過對UWB的測距信息進行解算，獲得位置坐標，計算平均值來作為估計值，計算動捕系統(tǒng)采集的數(shù)據(jù)求平均值作為真實值。記過如下圖所示。（a）四基站靜態(tài)定位（b）三基站靜態(tài)定位圖4-3UWB靜態(tài)定位效果圖上圖中，黑色三角形為基站的坐標，在四基站條件下，四個基站的坐標分別為：BS1（-2.328，-1.894），BS2（-2.337,2.358），BS3（2.353,2.337），BS4（2.321，-1.885）。在三基站條件下，三個基站的坐標分別為：BS1（-2.328，-1.894），BS2（-0.007,2.555），BS3（2.321，-1.885）。紅色方框為動捕系統(tǒng)的定位結(jié)果，藍色星型為UWB測距解算后的定位結(jié)果。從上圖可以看出來，當標簽距離基站的距離過近時，UWB的解算定位效果變差，四基站條件下定位結(jié)果最大誤差為（0.349，-0.448），均方根誤差RMSE為（0.094,0.124）。三基站條件下定位結(jié)果的最大誤差為（0.305，0.405），均方根誤差RMSE為（0.109,0.1218）。從結(jié)果上來說，三基站條件下和四基站條件下的靜態(tài)定位結(jié)果相差不大，而且可以看出，當標簽和基站的距離太近時，標簽的位置解算結(jié)果偏差較大。在后面的試驗中，應(yīng)該盡量保持標簽和基站之間的距離。4.2.2基于卡爾曼濾波的UWB定位系統(tǒng)動態(tài)試驗中，試驗場景的布置和靜態(tài)定位試驗的一致。在四基站條件下，四個基站布置在一個矩形場地的四個頂點上。在試驗中，遙控小車從圖4-4的左下角位置（-1.896，-1.574）出發(fā)，繞試驗場地順時鐘一周。其中，UWB的數(shù)據(jù)更新頻率為50Hz，IMU的數(shù)據(jù)更新頻率為100Hz，動作捕捉系統(tǒng)的更新頻率為100Hz。下圖為小車的運動軌跡。圖4-4基于卡爾曼濾波的定位結(jié)果圖4-5各方向定位結(jié)果上圖中，黑色曲線為動作捕捉系統(tǒng)的數(shù)據(jù)，作為真值使用。藍色曲線為最小二乘定位的結(jié)果，紅色曲線為卡爾曼濾波估計的結(jié)果。從圖中可以看出來，卡爾曼濾波對于定位結(jié)果有很好的平滑作用。圖4-6各方向定位誤差在上圖位置誤差曲線圖中可以看出，在采樣點800位置，x軸誤差出現(xiàn)一個尖峰毛刺，這是由于此時小車運行到基站附近，和前面的靜態(tài)試驗中分析的結(jié)果一致。4.2.3組合定位在視距環(huán)境中，基于卡爾曼濾波的UWB定位系統(tǒng)有較好的定位效果，但是當小車處于非視距環(huán)境中時，UWB會出現(xiàn)野值，導致定位精度下降。使用UWB和慣性導航進行組合導航可以有效的提高定位精度，提升系統(tǒng)的穩(wěn)定性。在組合定位試驗中，在小車的運行過程中加入遮擋物，來制造非視距環(huán)境。具體的試驗流程為，小車從左下角的起始點出發(fā)，運行軌跡為一個圓角矩形。當小車運行到矩形的上面這個邊時，在左下角這個基站前方加入一個障礙物，提供一個非視距環(huán)境。在下圖中的定位結(jié)果里面，也可以非常清楚的觀察到，當出現(xiàn)非視距環(huán)境時，UWB的測距結(jié)果會出現(xiàn)野值，反應(yīng)到定位結(jié)果中就是當出現(xiàn)非視距環(huán)境時，UWB的最小二乘定位中會出現(xiàn)很大的毛刺。圖4-7組合導航定位結(jié)果圖4-8組合導航定位結(jié)果細節(jié)圖圖4-9組合導航各定位效果上圖兩張圖分別為在二維平面中的定位結(jié)果和x軸和y軸的定位結(jié)果，在圖中，黑色曲線為動作捕捉系統(tǒng)的軌跡，作為真值來和定位效果進行比較；藍色曲線為根據(jù)UWB測距值通過最小二乘解算的定位結(jié)果；紅色曲線為對最小二乘定位的卡爾曼濾波估計結(jié)果；綠色曲線為通過擴展卡爾曼濾波將IMU的數(shù)據(jù)和UWB的數(shù)據(jù)進行融合后的定位結(jié)果；淡藍色曲線為通過自適應(yīng)卡爾曼濾波將IMU數(shù)據(jù)和UWB的數(shù)據(jù)進行融合后的定位結(jié)果。從上面的圖中可以了解到，當出現(xiàn)非視距環(huán)境時，由于UWB的測量結(jié)果出現(xiàn)了野值，這就使得最小二乘定位結(jié)果出現(xiàn)了毛刺，如圖5-11所示。為了減小最小二乘定位的誤差，使用卡爾曼濾波對結(jié)果進行估計，可以從圖中看出，紅色曲線在在非視距條件下的定位結(jié)果相較于最小二乘定位的結(jié)果提高了一點，但還是存在毛刺。為了提高定位精度，使用擴展卡爾曼濾波將IMU經(jīng)過轉(zhuǎn)換后的加速度數(shù)據(jù)和UWB的測距信息進行融合，可以從綠色曲線看出來，相較于前兩種定位方案，基于擴展卡爾曼濾波的組合定位的效果更好。當處于非視距環(huán)境中時，可以從上面兩張圖中可以觀察到，組合定位對于曲線有著更好的平滑效果。但是，也可以看出來，相較于視距環(huán)境，在非視距環(huán)境中的定位效果還是比較差。這是因為，在非視距條件時，量測噪聲的協(xié)方差矩陣R的值并沒有相應(yīng)的改變，還是使用視距條件下的量測噪聲矩陣。為了在非視距條件下得到更好的定位精度，使用Sage-Husa自適應(yīng)卡爾曼濾波將IMU經(jīng)過轉(zhuǎn)換后的加速度數(shù)據(jù)和UWB的測距信息進行融合?？梢詮牡{色曲線看出來，在非視距條件下，UWB測量結(jié)果的野值對自適應(yīng)卡爾曼濾波的定位結(jié)果的影響不大，相較于擴展卡爾曼濾波，加入了對參數(shù)進行估計的自適應(yīng)卡爾曼濾波有著更好的平滑作用。5結(jié)論本文先是分析了慣性導航系統(tǒng)的工作原理，介紹了幾種常用的坐標系，并分析了坐標系之間的轉(zhuǎn)換關(guān)系以及分析了捷聯(lián)慣導系統(tǒng)中的速度更新和位置更新。本文給出了定位結(jié)果的評價方法，包括圓概率誤差、最大定位誤差和均方根誤差。然后本文搭建了基于卡爾曼濾波的UWB定位系統(tǒng)，對比直接通過最小二乘方法來解算位置，定位精度有了明顯的提升；然后針對非視距環(huán)境，搭建了基于擴展卡爾曼濾波的UWB/IMU組合導航定位系統(tǒng)并進行了仿真試驗，還搭建了基于Sage-Husa自適應(yīng)卡爾曼濾波的組合導航定位系統(tǒng)，分析仿真結(jié)果后，提出了改進的簡化自適應(yīng)卡爾曼濾波的組合導航定位系統(tǒng)模型，相較于基于Sage-Husa自適應(yīng)卡爾曼濾波的組合導航定位系統(tǒng)在起始階段的收斂速度有了明顯的改善。最后設(shè)計搭建了基于LinkTrack-S和MTi-G-710組合導航定位系統(tǒng)的車載平臺。對比了基于測距信息的最小二乘法、基于基于卡爾曼濾波的UWB定位系統(tǒng)、基于擴展卡爾曼濾波的U