一種行人自主導(dǎo)航定位的自適應(yīng)卡爾曼濾波方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于導(dǎo)航定位技術(shù)領(lǐng)域，尤其涉及一種行人自主導(dǎo)航定位的自適應(yīng)卡爾曼 濾波方法。
【背景技術(shù)】
[0002] 近些年來(lái)導(dǎo)航定位技術(shù)依然在迅猛發(fā)展，而其中多數(shù)是基于衛(wèi)星的導(dǎo)航系統(tǒng)，并 且只適用于室外的環(huán)境，如應(yīng)用最廣泛的GPS，在城市樓群、山區(qū)、森林和地下建筑等遮擋環(huán) 境下，其信號(hào)弱，可用性差。
[0003] 隨著人們生活節(jié)奏的加快，行人自主導(dǎo)航定位的需求也顯得越發(fā)迫切。尤其是 室內(nèi)環(huán)境下，如火災(zāi)、樓宇坍塌等緊急環(huán)境的救援、商場(chǎng)內(nèi)需要尋人或某個(gè)地方等。很多 人采用地圖信息匹配的方法進(jìn)行行人導(dǎo)航，如國(guó)外專(zhuān)利CN1705861A《Walker navigation device and program》，其利用存儲(chǔ)在地圖數(shù)據(jù)庫(kù)中的地圖信息來(lái)計(jì)算當(dāng)前位置并顯示。這 種方法需要先驗(yàn)的預(yù)知信息，對(duì)于火災(zāi)救援等臨時(shí)突發(fā)緊急情況難以達(dá)到良好的處理效 果。本發(fā)明中應(yīng)用MEMS-頂U(kuò)固連于人體對(duì)行人進(jìn)行室內(nèi)實(shí)時(shí)定位，不需要依賴(lài)任何外部的 信息，可以實(shí)現(xiàn)全自主定位，并且價(jià)格便宜，體積小，容易實(shí)現(xiàn)可穿戴。利用慣性傳感器實(shí)現(xiàn) 行人定位的經(jīng)典方法是行人航位推算（PDR)，其中大體可分為兩類(lèi)：一種是通過(guò)一步計(jì)數(shù) 和步長(zhǎng)估計(jì)來(lái)實(shí)現(xiàn)定位，通常用陀螺儀/羅盤(pán)來(lái)增強(qiáng)航向信息。如專(zhuān)利CN201310388466. 9 《行人步長(zhǎng)估計(jì)及航位推算方法》中所述，利用步數(shù)、步長(zhǎng)、方向進(jìn)行測(cè)量和計(jì)算。但是此 種方式往往只適用于特定的用戶(hù)，在不尋常的行走模式，包括擁擠環(huán)境、上坡/下坡等環(huán)境 中，步行統(tǒng)計(jì)假設(shè)條件會(huì)被破壞，因此容易失敗。另一種方法是采用直接積分策略，但是隨 著行人移動(dòng)距離和航向角的增加，慣性器件累計(jì)誤差會(huì)越來(lái)越大，導(dǎo)致其定位精度不理想。 在考慮系統(tǒng)實(shí)時(shí)性及計(jì)算復(fù)雜度的前提下，通常采用卡爾曼濾波來(lái)消除漂移誤差。
[0004] 現(xiàn)有的用于行人導(dǎo)航系統(tǒng)的濾波方法主要采用零速修正的方法，而后續(xù)跟進(jìn)的濾 波一般均采取基礎(chǔ)的經(jīng)典卡爾曼濾波或擴(kuò)展卡爾曼濾波。如國(guó)內(nèi)編號(hào)為CN201310566710 的專(zhuān)利《基于捷聯(lián)慣導(dǎo)解算和零速校正的自主導(dǎo)航系統(tǒng)定位方法》中所用的是經(jīng)典卡爾曼 濾波，其通過(guò)判斷人腳處于運(yùn)動(dòng)還是靜止?fàn)顟B(tài)來(lái)區(qū)分更新選項(xiàng)。當(dāng)檢測(cè)為運(yùn)動(dòng)時(shí)，只啟動(dòng)時(shí) 間更新；當(dāng)檢測(cè)為靜止時(shí)，則同時(shí)啟動(dòng)時(shí)間更新和量測(cè)更新。經(jīng)典卡爾曼濾波是將系統(tǒng)默認(rèn) 為線性系統(tǒng)來(lái)進(jìn)行更新處理，而人體運(yùn)動(dòng)必然會(huì)伴有擺動(dòng)等干擾因素，顯然是非線性系統(tǒng)， 所以若應(yīng)用經(jīng)典卡爾曼濾波進(jìn)行估計(jì)，修正效果并不理想。再如專(zhuān)利CN201310520233《一 種基于雙MEMS-頂U(kuò)的行人自主導(dǎo)航解算算法》中以雙腳解算距離超過(guò)雙腳間最大步長(zhǎng)為觸 發(fā)點(diǎn)，利用EKF完成估計(jì)修正，即用導(dǎo)航解算的協(xié)方差陣來(lái)更新卡爾曼濾波的協(xié)方差陣。此 方法是將系統(tǒng)局部線性化，比起經(jīng)典卡爾曼濾波的方法更符合行人運(yùn)動(dòng)的實(shí)際情況，因此 定位精度有所提升，但仍然沒(méi)解決噪聲實(shí)時(shí)變化的影響問(wèn)題，并且其每次更新都必須通過(guò) 計(jì)算雅可比矩陣才能完成，實(shí)時(shí)性變差。
[0005] 綜上所述，在某種程度上本發(fā)明從成本、可靠性、復(fù)雜性以及精確性等角度都表現(xiàn) 出了其它相關(guān)研究不具備或不同時(shí)具備的新穎性和先進(jìn)性。

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

[0006] 本發(fā)明所要解決的技術(shù)問(wèn)題在于克服現(xiàn)有技術(shù)的不足，提供一種行人自主導(dǎo)航定 位的自適應(yīng)卡爾曼濾波方法，該方法簡(jiǎn)單、穩(wěn)定性強(qiáng)，在不增加任何額外硬件成本的情況 下，有效提1? 了彳丁人導(dǎo)航定位的精度。
[0007] 本發(fā)明采用以下技術(shù)方案：
[0008] -種行人自主導(dǎo)航定位的自適應(yīng)卡爾曼濾波方法，包括以下步驟：
[0009] 步驟（1)，將MEMS-頂U(kuò)系統(tǒng)固定連接于人體，頂U(kuò)初始對(duì)準(zhǔn)后，行人開(kāi)始運(yùn)動(dòng)，運(yùn)動(dòng) 過(guò)程中采集IMU數(shù)據(jù)；
[0010] 步驟（2)，合理選取觀測(cè)量建立融合人體運(yùn)動(dòng)特征的自適應(yīng)卡爾曼濾波模型；
[0011] 步驟（3)，同時(shí)滿(mǎn)足"四條件"時(shí)進(jìn)行自適應(yīng)卡爾曼濾波，同步修正晃動(dòng)噪聲干擾， 完成各狀態(tài)量的估計(jì)。
[0012] 步驟（4)，利用估計(jì)誤差修正運(yùn)動(dòng)人體信息。
[0013] 進(jìn)一步的，在步驟（1)中，頂U(kuò)輸出數(shù)據(jù)是通過(guò)固連于人體上的測(cè)量傳感器件測(cè)量 所得；測(cè)量傳感器件包括三軸加速度計(jì)、三軸陀螺儀和三軸磁力計(jì)。
[0014] 進(jìn)一步的，在步驟（1)中的初始對(duì)準(zhǔn)選取北東地為導(dǎo)航坐標(biāo)系，俯仰角Θ和滾轉(zhuǎn) 角Y由經(jīng)過(guò)補(bǔ)償后的加速度計(jì)輸出值計(jì)算獲得，偏航角供由磁場(chǎng)強(qiáng)度B、俯仰角Θ和滾轉(zhuǎn) 角Y計(jì)算獲得。計(jì)算公式為：
[0015] 其中，k代表k時(shí)刻，b代表載體（行人）坐標(biāo)系，η代表導(dǎo)航坐標(biāo)系，4表示k時(shí) 刻載體坐標(biāo)系下行人的加速度，S4"表示k時(shí)刻導(dǎo)航坐標(biāo)系下的磁場(chǎng)強(qiáng)度。
[0016] 進(jìn)一步的，在步驟（2)中，濾波模型為包括3個(gè)位置誤差、3個(gè)姿態(tài)角誤差（俯仰角 Θ、滾轉(zhuǎn)角Y和偏航角P )、3個(gè)速度誤差、3個(gè)重力不確定誤差、三軸陀螺儀零偏誤差和三 軸加速度計(jì)零偏誤差，共計(jì)18維的狀態(tài)空間方程。
[0018] W(t)為系統(tǒng)過(guò)程噪聲矩陣，其表達(dá)式為：
；G(t)為 對(duì)應(yīng)的噪聲矩陣系數(shù)；F(t)為系統(tǒng)狀態(tài)矩陣；Z(t)為觀測(cè)量；H(t)為觀測(cè)矩陣；V(t)為觀 測(cè)噪聲矩陣。
[0019] 被估計(jì)狀態(tài)向量為：
[0020] 其中，δ r為位置誤差，δ V為速度誤差，δ ψ為姿態(tài)角誤差，\^為加速度計(jì)零偏 誤差，eb為陀螺儀零偏誤差，Sg為重力不確定誤差。
[0021] 觀測(cè)向量和觀測(cè)矩陣為：
[0022] Zk= [Avk Δω, Δ ψ,]τ
[0024] 其中，Δ urk包括磁力計(jì)計(jì)算的航向角與捷聯(lián)解算出的航向角作差獲得的航向角 誤差，利用加速度計(jì)計(jì)算出的俯仰角和橫滾角與捷聯(lián)解算出的姿態(tài)角作差獲得的俯仰角和 橫滾角誤差。
[0025] 進(jìn)一步的，所述的步驟（3)中的"四條件"包括：
[0026] (1)三軸加速度計(jì)合成的加速度幅值輸出量滿(mǎn)足閾值條件；
[0027] (2)加速度計(jì)z軸加速度輸出量滿(mǎn)足閾值條件；
[0028] (3)三軸陀螺儀合成的角速度幅值輸出量滿(mǎn)足閾值條件；
[0029] (4)陀螺儀y軸角速度輸出量滿(mǎn)足閾值條件；
[0030] 當(dāng)上述四項(xiàng)條件同時(shí)滿(mǎn)足時(shí)，進(jìn)行自適應(yīng)濾波。
[0031] 其中，條件（1)中三軸加速度計(jì)合成的加速度幅值表示為
，定義給定的閾值條件為th amin= 9m/ s2< I a k I < th_x= Ilm/s 2。條件（3)中三軸陀螺儀合成的角速度幅值表示為
，定義給定的閾值條件為I ?k| < th"max= 50° /s〇
[0032] 在行人運(yùn)動(dòng)過(guò)程中，z軸加速度和y軸角速度變化最明顯，由于安裝偏差、行走晃 動(dòng)等影響，頂U(kuò)會(huì)有所傾斜。因此在靜止時(shí)刻y軸角速度不是完全零值，z軸加速度也不是 完全g值，但都不會(huì)大于所設(shè)定閾值。條件（2)和條件（4)中的閾值在初始靜止階段分別 以取加速度計(jì)輸出的平均值和陀螺儀輸出的平均值而設(shè)定。
[0033] 進(jìn)一步的，在步驟（3)中，采用自適應(yīng)卡爾曼濾波進(jìn)行誤差估計(jì)，利用觀測(cè)數(shù)據(jù) 進(jìn)行遞推濾波的同時(shí)，通過(guò)時(shí)變?cè)肼暯y(tǒng)計(jì)估計(jì)器實(shí)時(shí)估計(jì)和修正系統(tǒng)噪聲以及觀測(cè)噪聲的 統(tǒng)計(jì)特性，從而降低模型誤差、抑制濾波發(fā)散，提高濾波精度。選取遺忘因子b = 0.98。
[0034] 進(jìn)一步的，在步驟（4)中，通過(guò)公式 反饋修正行人速度、位置和姿態(tài)信息。
[0035] 本發(fā)明比現(xiàn)有技術(shù)具有以下有益效果及優(yōu)點(diǎn)：
[0036] (1)本發(fā)明可穿戴，不需要預(yù)設(shè)節(jié)點(diǎn)，比其它需要預(yù)先知道室內(nèi)布局環(huán)境的定位方 法更方便，適用性更廣。另外，相比于其它基于WiFi、Zigbee等無(wú)線網(wǎng)絡(luò)的室內(nèi)定位技術(shù)， 本發(fā)明穩(wěn)定性更高。
[0037] (2)本發(fā)明針對(duì)人體運(yùn)動(dòng)特性，采用的"四條件"識(shí)別方法簡(jiǎn)單，實(shí)時(shí)性高，起到良 好的觸發(fā)作用。比應(yīng)用步長(zhǎng)限制條件判斷零瞬態(tài)的方法應(yīng)用范圍更廣，不針對(duì)指定用戶(hù)。
[0038] (3)在不增加任何額外硬件成本的情況下，利用自適應(yīng)卡爾曼濾波器解決慣性器 件長(zhǎng)時(shí)間工作產(chǎn)生的累積漂移影響定位精度的問(wèn)題。自適應(yīng)體現(xiàn)在利用觀測(cè)數(shù)據(jù)進(jìn)行遞推 濾波的同時(shí)，用時(shí)變?cè)肼暯y(tǒng)計(jì)估計(jì)器實(shí)時(shí)估計(jì)和修正系統(tǒng)噪聲以及觀測(cè)噪聲的統(tǒng)計(jì)特性， 從而降低模型誤差、抑制濾波發(fā)散，提高濾波精度。此方法針對(duì)行人運(yùn)動(dòng)這種噪聲實(shí)時(shí)變換 的情況有良好的效果。
【附圖說(shuō)明】
[0039] 圖1為本發(fā)明的流程圖
[0040] 圖2為本發(fā)明的工作原理圖；
[0041] 圖3為自適應(yīng)卡爾曼濾波框圖；
[0042] 圖4為行人自主導(dǎo)航定位的自適應(yīng)卡爾曼濾波方法實(shí)施例結(jié)果對(duì)比圖。
【具體實(shí)施方式】
[0043] 下面結(jié)合附圖和【具體實(shí)施方式】對(duì)本發(fā)明作進(jìn)一步描述：
[0044] 圖1是本發(fā)明的流程圖，其中，數(shù)據(jù)采集的頂U(kuò)模塊（1-1)包括加速度計(jì)、陀螺 儀和磁力計(jì)。將數(shù)據(jù)進(jìn)行捷聯(lián)慣性導(dǎo)航解算（1-2)，之后選取合適觀測(cè)量得到濾波模型 (1-3)，隨后用"四條件"觸發(fā)AKF模塊（1-4)進(jìn)行濾波。人體在正常運(yùn)動(dòng)過(guò)程中，與地面接 觸時(shí)間為毫秒級(jí)或更長(zhǎng)，其中某一時(shí)刻人體與地面接觸會(huì)達(dá)到瞬間靜止?fàn)顟B(tài)，當(dāng)同時(shí)滿(mǎn)足 所述"四條件"時(shí)即為瞬間靜止?fàn)顟B(tài)。AKF指自適應(yīng)卡爾曼濾波，AKF模塊實(shí)現(xiàn)對(duì)系統(tǒng)噪聲 以及觀測(cè)噪聲統(tǒng)計(jì)特性的實(shí)時(shí)估計(jì)，從而降低模型誤差、抑制濾波發(fā)散，提高濾波精度。最 后，用估計(jì)量完成定位信息的校正（1-5)。
[0045] 圖2是本發(fā)明的工作原理圖。其中，MEMS-頂U(kuò)(2-1)包括三軸加速度計(jì)、三軸陀螺 儀和三軸磁力計(jì)。加速度計(jì)輸出加速度信息（2-2)，陀螺儀輸出角速度信息（2-3)，將以上 兩種直接輸出信息進(jìn)行捷聯(lián)慣性導(dǎo)航解算（2-4)，可得到位置、速度、姿態(tài)信息（2-5)。當(dāng)檢 測(cè)到行人處于瞬間靜止?fàn)顟B(tài)（2-7)，即觸發(fā)濾波器，以零速度修正量（2-8)、零角速度修正 量（2-9)、姿態(tài)角修正量（2-6)同時(shí)成為觀測(cè)量，利用自適應(yīng)卡爾曼濾波器（2-10)進(jìn)行估 計(jì)，隨后進(jìn)行反饋修正（2-11)，得到最終輸出的位置、速度、姿態(tài)信息（2-12)。
[0046] -種行人自主導(dǎo)航定位的自適應(yīng)卡爾曼濾波方