一種基于自適應(yīng)卡爾曼濾波的行人航向最優(yōu)融合方法技術(shù)

本發(fā)明專利技術(shù)公開了一種基于自適應(yīng)卡爾曼濾波器的行人航向最優(yōu)融合方法，屬于行人導(dǎo)航技術(shù)領(lǐng)域。本方法構(gòu)建了9階自適應(yīng)卡爾曼濾波算法模型，每到量測時刻，融合捷聯(lián)航向角與磁航向角，利用磁傳感器實時統(tǒng)計信息，建立磁異常辨識模型，進而在不同的辨識狀態(tài)下實時修正自適應(yīng)卡爾曼濾波量測噪聲陣，實現(xiàn)捷聯(lián)航向角與磁航向角的最優(yōu)融合，保證組合航向角的高精度。本發(fā)明專利技術(shù)采用低精度的消費級傳感器芯片，無論室內(nèi)外，均能有效的保證行人航向角高精度，實用性較強。

【技術(shù)實現(xiàn)步驟摘要】
一種基于自適應(yīng)卡爾曼濾波的行人航向最優(yōu)融合方法
本專利技術(shù)涉及一種基于自適應(yīng)卡爾曼濾波的行人航向最優(yōu)融合方法，屬于行人導(dǎo)航

技術(shù)介紹
行人導(dǎo)航技術(shù)已掀起國內(nèi)外研究熱潮，其中航向作為核心技術(shù)之一，對行人導(dǎo)航定位精度起著至關(guān)重要的作用。目前較為普遍的做法是利用陀螺儀獲得角速率，進而四元素解算得到角度，但是低成本的慣性傳感器，其誤差較大且發(fā)散嚴(yán)重，長時間內(nèi)將會導(dǎo)致行人定位失效。地磁場作為地球的固有屬性，可以用于計算磁航向角，但是室內(nèi)地磁受到鋼筋、管道等其他金屬制品的影響，會產(chǎn)生較大的誤差，無法滿足所有路段高精度航向的要求。目前，國內(nèi)外主要考察磁航向角與陀螺解算出的角度的差值大小，當(dāng)這一差值大于某一預(yù)定閾值時，即為判定有磁場異常情況。但是這種判別方法受限于磁航向角的解算精度，本身磁傳感器尚未標(biāo)定完全會帶有誤差，投影解算航向時又會引入二次誤差，判別精度較低。同時針對磁航向角與捷聯(lián)解算出來的航向角融合方面，其僅僅只有完全融合與完全不融合兩種情況，其靈活性與適應(yīng)性較差，當(dāng)磁異常情況微弱時，可以嘗試提取有效信息進行航向融合。
技術(shù)實現(xiàn)思路
本專利技術(shù)采用低精度的陀螺傳感器與磁傳感器，提出了一種基于自適應(yīng)卡爾曼濾波的行人航向最優(yōu)融合方法，解決不同磁環(huán)境下的航向融合問題。利用磁異常辨識算法有效區(qū)分磁異常環(huán)境，進而在不同環(huán)境下恰當(dāng)?shù)倪x取量測噪聲陣，構(gòu)建自適應(yīng)卡爾曼濾波器靈活實現(xiàn)捷聯(lián)航向角與磁航向角的最優(yōu)融合，實現(xiàn)室內(nèi)外不同環(huán)境下行人航向的高精度。本專利技術(shù)為解決其技術(shù)問題采用如下技術(shù)方案：一種基于自適應(yīng)卡爾曼濾波的行人航向最優(yōu)融合方法，包括如下步驟：步驟1，首先建立自適應(yīng)卡爾曼濾波狀態(tài)方程，選用“東北天”地理坐標(biāo)系，構(gòu)建9階狀態(tài)模型，如下式所示其中為東向平臺角誤差；為北向平臺角誤差；為天向平臺角誤差；εbx為x軸陀螺隨機常數(shù)；εby為y軸陀螺隨機常數(shù)；εbz為z軸陀螺隨機常數(shù)；εrx為x軸陀螺一階馬爾科夫過程；εry為y軸陀螺一階馬爾科夫過程；εrz為z軸陀螺一階馬爾科夫過程；W為系統(tǒng)隨機過程噪聲序列；A為系統(tǒng)矩陣；G為系統(tǒng)噪聲矩陣；W為系統(tǒng)噪聲序列；X為狀態(tài)量；為狀態(tài)量導(dǎo)數(shù)；wgx為x軸隨機白噪聲驅(qū)動；wgy為y軸隨機白噪聲驅(qū)動；wgz為z軸隨機白噪聲驅(qū)動；wrx為x軸馬爾科夫白噪聲驅(qū)動；wry為y軸馬爾科夫白噪聲驅(qū)動；wrz為z軸馬爾科夫白噪聲驅(qū)動；步驟2，在步驟1自適應(yīng)卡爾曼濾波狀態(tài)方程建立好的基礎(chǔ)之上，開始導(dǎo)航；利用陀螺儀每0.005秒采集一次數(shù)據(jù)，經(jīng)誤差修正后，通過四元素解算得出當(dāng)前捷聯(lián)航向角，利用誤差修正后的磁傳感器信息解算出當(dāng)前磁航向角；步驟3，在步驟2的基礎(chǔ)上，判斷當(dāng)前解算時間是否達到1秒，無則返回步驟2，有則進行步驟4；步驟4，在步驟3的基礎(chǔ)上，研究利用磁傳感器實時統(tǒng)計信息進行磁異常辨識，再利用二維橢圓標(biāo)定算法修正磁傳感器信息之后，按照下式構(gòu)造磁異常辨識模型上式代表第k時刻的磁環(huán)境辨識情況，其中，σ3為第一種磁異常判別參數(shù)；σ4為第二種磁異常判別參數(shù)；var()代表方差函數(shù)；min()代表求取最小值函數(shù)；mag1為第一個時刻總磁場強度；代表窗口大小為N的滑動數(shù)組；N為數(shù)組大小；數(shù)組存儲總磁場強度并實時更新，總磁場強度如下式所示，由三軸磁傳感器測量值的平方和開根號組成。其中：magi為第i時刻總磁場強度；magix為x軸磁場強度；magiy為y軸磁場強度；magiz為z軸磁場強度；步驟5：在步驟4的基礎(chǔ)上，進行自適應(yīng)卡爾曼濾波量測建模，構(gòu)建一維量測模型，量測周期為1秒，量測方程如下式所示：其中，為捷聯(lián)解算出的航向角；為磁航向角；為姿態(tài)誤差角；為航向噪聲；Hv(t)為量測矩陣；X(t)為狀態(tài)量；Vv(t)為量測噪聲；磁航向角解算公式如下式所示，其中X、Y分別為前向和橫側(cè)向所測量得到的磁信息。姿態(tài)誤差角與平臺誤差角存在如下關(guān)系：其中，θ為俯仰角，φx、φy、φy為三個平臺誤差角。因此：同時利用步驟4的結(jié)果，依據(jù)下式對卡爾曼濾波器量測噪聲陣Vv(t)進行修正。其中λ為設(shè)定的經(jīng)驗值；η為事先人為設(shè)定的增益常量；ε3與ε4為通過統(tǒng)計學(xué)方法獲得的參數(shù)值，即在磁正常環(huán)境下由式(2)獲得的均值。步驟6：在步驟5的基礎(chǔ)上，利用上述自適應(yīng)卡爾曼濾波器對組合航向角進行修正，同時將陀螺估計誤差反饋給陀螺誤差修正模型，并返回步驟2。本專利技術(shù)的有益效果如下：1.采用低精度的消費級傳感器芯片，無論室內(nèi)外，均能有效的保證行人航向角高精度，實用性較強。2.解決了磁異常環(huán)境下磁航向角誤用的問題。3.傳感器成本低且普及較廣，算法的實用性與推廣性較強。附圖說明圖1為自適應(yīng)卡爾曼濾波的行人航向最優(yōu)融合流程示意圖。具體實施方案下面結(jié)合附圖對本專利技術(shù)創(chuàng)造做進一步詳細說明。如圖1所示，導(dǎo)航解算過程中，陀螺儀可以實時獲得角速率信息，通過四元素解算，實時獲得捷聯(lián)航向角，解算頻率一般可設(shè)置為50Hz。磁傳感器實時采集磁信息，主要分為兩部分，一部分用于解算出磁航向角，一部分用于磁異常辨識，實時靈活調(diào)整量測噪聲陣。以捷聯(lián)航向角為主，每過1秒，利用自適應(yīng)卡爾曼濾波器，利用磁航向角建立觀測方程，實時修正捷聯(lián)航向角誤差，提高航向角精度，并返回估計誤差對慣性傳感器進行誤差修正。基于自適應(yīng)卡爾曼濾波的行人航向最優(yōu)融合算法主要包括以下三個步驟：步驟一：自適應(yīng)卡爾曼濾波器狀態(tài)建模選用“東北天”地理坐標(biāo)系，構(gòu)建9階狀態(tài)模型，如式(1)所示。其中為東向平臺角誤差；為北向平臺角誤差；為天向平臺角誤差；εbx為x軸陀螺隨機常數(shù)；εby為y軸陀螺隨機常數(shù)；εbz為z軸陀螺隨機常數(shù)；εrx為x軸陀螺一階馬爾科夫過程；εry為y軸陀螺一階馬爾科夫過程；εrz為z軸陀螺一階馬爾科夫過程；W為系統(tǒng)隨機過程噪聲序列；A為系統(tǒng)矩陣；G為系統(tǒng)噪聲矩陣；W為系統(tǒng)噪聲序列；X為狀態(tài)量；為狀態(tài)量導(dǎo)數(shù)；wgx為x軸隨機白噪聲驅(qū)動；wgy為y軸隨機白噪聲驅(qū)動；wgz為z軸隨機白噪聲驅(qū)動；wrx為x軸馬爾科夫白噪聲驅(qū)動；wry為y軸馬爾科夫白噪聲驅(qū)動；wrz為z軸馬爾科夫白噪聲驅(qū)動。步驟二：磁異常辨識模型研究利用磁傳感器實時統(tǒng)計信息進行磁異常辨識，磁異常環(huán)境下，無論是磁場強度模值還是磁場強度變化趨勢，均與磁正常環(huán)境有著較大差異。再利用二維橢圓標(biāo)定算法修正磁傳感器信息之后，按照(2)式構(gòu)造磁異常辨識模型。上式代表第k時刻的磁環(huán)境辨識情況。其中，σ3為第一種磁異常判別參數(shù)；σ4為第二種磁異常判別參數(shù)；var()代表方差函數(shù)；min()代表求取最小值函數(shù)；mag1為第一時刻總磁場強度；代表窗口大小為N的滑動數(shù)組；N為數(shù)組大小；數(shù)組存儲總磁場強度并實時更新，總磁場強度如式(3)所示，由三軸磁傳感器測量值的平方和開根號組成。其中：magi為第i時刻總磁場強度；magix為x軸磁場強度大小；magiy為y軸磁場強度大小；magiz為z軸磁場強度大小。步驟三：自適應(yīng)卡爾曼濾波器量測建模構(gòu)建一維量測模型，量測周期為1秒，量測方程如下式所示：其中，為捷聯(lián)解算出的航向角；為磁航向角；為姿態(tài)誤差角；為航向噪聲；Hv(t)為量測矩陣；X(t)為狀態(tài)量；Vv(t)為量測噪聲。磁航向角解算公式如下式(5)所示，其中X、Y分別為前向和橫側(cè)向所測量得到的磁信息。姿態(tài)誤差角與平臺誤差角存在如下關(guān)系：其中，θ為俯仰角，φx、φy、φy為三個平臺本文檔來自技高網(wǎng)...

【技術(shù)保護點】
一種基于自適應(yīng)卡爾曼濾波的行人航向最優(yōu)融合方法，其特征在于，包括如下步驟：步驟1，首先建立自適應(yīng)卡爾曼濾波狀態(tài)方程，選用“東北天”地理坐標(biāo)系，構(gòu)建9階狀態(tài)模型，如下式所示

【技術(shù)特征摘要】
1.一種基于自適應(yīng)卡爾曼濾波的行人航向最優(yōu)融合方法，其特征在于，包括如下步驟：步驟1，首先建立自適應(yīng)卡爾曼濾波狀態(tài)方程，選用“東北天”地理坐標(biāo)系，構(gòu)建9階狀態(tài)模型，如下式所示其中為東向平臺角誤差；為北向平臺角誤差；為天向平臺角誤差；εbx為x軸陀螺隨機常數(shù)；εby為y軸陀螺隨機常數(shù)；εbz為z軸陀螺隨機常數(shù)；εrx為x軸陀螺一階馬爾科夫過程；εry為y軸陀螺一階馬爾科夫過程；εrz為z軸陀螺一階馬爾科夫過程；W為系統(tǒng)隨機過程噪聲序列；A為系統(tǒng)矩陣；G為系統(tǒng)噪聲矩陣；W為系統(tǒng)噪聲序列；X為狀態(tài)量；為狀態(tài)量導(dǎo)數(shù)；wgx為x軸隨機白噪聲驅(qū)動；wgy為y軸隨機白噪聲驅(qū)動；wgz為z軸隨機白噪聲驅(qū)動；wrx為x軸馬爾科夫白噪聲驅(qū)動；wry為y軸馬爾科夫白噪聲驅(qū)動；wrz為z軸馬爾科夫白噪聲驅(qū)動；步驟2，在步驟1自適應(yīng)卡爾曼濾波狀態(tài)方程建立好的基礎(chǔ)之上，開始導(dǎo)航；利用陀螺儀每0.005秒采集一次數(shù)據(jù)，經(jīng)誤差修正后，通過四元素解算得出當(dāng)前捷聯(lián)航向角，利用誤差修正后的磁傳感器信息解算出當(dāng)前磁航向角；步驟3，在步驟2的基礎(chǔ)上，判斷當(dāng)前解算時間是否達到1秒，無則返回步驟2，有則進行步驟4；步驟4，在步驟3的基礎(chǔ)上，研究利用磁傳感器實時統(tǒng)計信息進行磁異常辨識，再利用二維橢圓標(biāo)定算法修正磁傳感器信息之后，按照下式構(gòu)造磁異常辨識模型

【專利技術(shù)屬性】
技術(shù)研發(fā)人員：黃欣，熊智，許建新，徐麗敏，孔雪博，趙宣懿，萬眾，李一博，
申請(專利權(quán))人：南京航空航天大學(xué)，
類型：發(fā)明
國別省市：江蘇,32