一種基于雙向?yàn)V波平滑技術(shù)的步進(jìn)式行人導(dǎo)航方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及行人導(dǎo)航定位技術(shù)領(lǐng)域，尤其涉及一種基于雙向?yàn)V波平滑技術(shù)的步進(jìn) 式行人導(dǎo)航方法。
【背景技術(shù)】
[0002] 目前，市場(chǎng)上基于GPS技術(shù)的定位設(shè)備發(fā)展迅速，市場(chǎng)占有率呈逐年上升趨勢(shì)。但 GPS技術(shù)易受工作環(huán)境的干擾，只適于室外沒有遮擋的開闊環(huán)境，在室內(nèi)、叢林、井下和洞穴 等區(qū)域存在導(dǎo)航盲區(qū)，無(wú)法正常工作。因此，基于慣性傳感器的行人導(dǎo)航技術(shù)得到了快速的 發(fā)展，主要用于確定行人的實(shí)時(shí)位置和記錄行人的行走軌跡，實(shí)現(xiàn)行人定位和導(dǎo)航的功能。
[0003] 近年來(lái)，隨著微機(jī)電（Micro-Electro-Mechanical Systems，MEMS)技術(shù)的不斷發(fā) 展，基于MEMS技術(shù)的慣性傳感器（Inertial Measurement Unit，IMU)也有了很大的改進(jìn)。 MEMS IMU具有體積小、質(zhì)量輕、功耗低、響應(yīng)快、成本低、靈敏度高等優(yōu)點(diǎn)，成為穿戴式行人 導(dǎo)航技術(shù)的理想選擇。
[0004] 但是，市場(chǎng)上常見的MEMS MU的精度大致在商業(yè)級(jí)和戰(zhàn)術(shù)級(jí)水平，存在多種隨機(jī) 誤差和系統(tǒng)誤差。而慣導(dǎo)技術(shù)是一種相對(duì)定位技術(shù)，導(dǎo)航誤差具有隨時(shí)間不斷積累的特點(diǎn)。 若單獨(dú)基于MEMS MU進(jìn)行純慣導(dǎo)解算，導(dǎo)航結(jié)果的位置誤差會(huì)以導(dǎo)航時(shí)間三次方的趨勢(shì)迅 速發(fā)散，嚴(yán)重偏離實(shí)際位置，使系統(tǒng)逐漸喪失導(dǎo)航功能，最終導(dǎo)致導(dǎo)航任務(wù)失敗。這種固有 的誤差發(fā)散特性，使得MEMS IMU在實(shí)際的導(dǎo)航應(yīng)用中受到很大的限制，其應(yīng)用價(jià)值一度受 到質(zhì)疑。因此，在目前MEMS MU技術(shù)的發(fā)展水平下，對(duì)導(dǎo)航誤差的處理就顯得異常重要。
[0005] 基于 MEMS IMU 的行人導(dǎo)航系統(tǒng)（Personal Navigation System，PNS)是在傳統(tǒng)捷 聯(lián)慣導(dǎo)系統(tǒng)（Strapdown Inertial Navigation System，SINS)的基礎(chǔ)上，利用人體行走時(shí) 足部運(yùn)動(dòng)特有的周期特性，通過(guò)零速校正（Zero Velocity Updates，ZUPT)算法，實(shí)現(xiàn)導(dǎo)航 誤差的估計(jì)和校正?；诳柭鼮V波器（Kalman Filter，KF)的零速校正算法，充分利用了 速度與姿態(tài)和位置之間的耦合關(guān)系，在消除速度誤差的同時(shí)，可以估計(jì)和校正包含姿態(tài)誤 差和位置誤差在內(nèi)的更多其他導(dǎo)航誤差。而基于單獨(dú)前向卡爾曼濾波器的零速校正算法只 在每個(gè)步態(tài)周期的支撐相內(nèi)進(jìn)行誤差估計(jì)和校正，無(wú)法對(duì)擺動(dòng)相內(nèi)的誤差進(jìn)行及時(shí)有效的 估計(jì)和校正，導(dǎo)致擺動(dòng)相內(nèi)系統(tǒng)處于自由導(dǎo)航模式，導(dǎo)航誤差迅速積累和傳播，未得到任何 有效地抑制和約束，而且會(huì)在系統(tǒng)從擺動(dòng)相過(guò)渡到支撐相時(shí)引起導(dǎo)航信息的突變，導(dǎo)致導(dǎo) 航狀態(tài)的不連讀，尤其是當(dāng)步態(tài)周期的支撐相不能被正確檢測(cè)時(shí)，導(dǎo)航信息的突變更加明 顯，導(dǎo)致所估計(jì)的導(dǎo)航狀態(tài)偏離其真實(shí)值。若加入平滑處理算法，又會(huì)引入算法延時(shí)，增加 算法的時(shí)間復(fù)雜度，加大對(duì)存儲(chǔ)空間的需求。

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

[0006] 由于現(xiàn)有技術(shù)中基于單向閉環(huán)卡爾曼濾波器的零速校正算法，只在每個(gè)步態(tài)周期 的支撐相內(nèi)進(jìn)行誤差估計(jì)和校正，無(wú)法對(duì)擺動(dòng)相內(nèi)的誤差進(jìn)行估計(jì)，在從擺動(dòng)相過(guò)渡到支 撐相時(shí)會(huì)引起導(dǎo)航信息的突變，尤其是當(dāng)步態(tài)周期的支撐相不能被正確檢測(cè)時(shí)，導(dǎo)航信息 的突變更加明顯，導(dǎo)致行人導(dǎo)航方法所估計(jì)的導(dǎo)航狀態(tài)嚴(yán)重偏離其真實(shí)值。因此，本發(fā)明提 出了一種基于雙向?yàn)V波平滑技術(shù)的步進(jìn)式行人導(dǎo)航方法，來(lái)提行人導(dǎo)航方法的精確性、魯 棒性和平滑性。
[0007] 雙向?yàn)V波平滑算法由前向卡爾曼濾波算法和后向固定區(qū)間平滑算法構(gòu)成，在實(shí)際 的準(zhǔn)實(shí)時(shí)應(yīng)用中，前向?yàn)V波與后向平滑分開執(zhí)行。以每個(gè)步態(tài)周期為區(qū)間，先執(zhí)行前向卡爾 曼濾波算法，再執(zhí)行后向固定區(qū)間平滑算法。雖然后向平滑算法需要對(duì)整個(gè)步態(tài)周期內(nèi)的 誤差進(jìn)行逐點(diǎn)平滑估計(jì)，但為了確保傳感器采樣數(shù)據(jù)的連續(xù)性，所有誤差估計(jì)、平滑和校正 的操作只能在傳感器連續(xù)兩次數(shù)據(jù)采樣之間的采樣間隔內(nèi)進(jìn)行，即誤差處理時(shí)間受限于傳 感器的采樣頻率和行人的行走步頻。然而后向平滑算法的引入，需要保存前向?yàn)V波的狀態(tài) 估計(jì)值、狀態(tài)估計(jì)誤差協(xié)方差矩陣、狀態(tài)轉(zhuǎn)移矩陣和觀測(cè)矩陣等，加大了對(duì)內(nèi)存的需求，增 加了算法的復(fù)雜度和計(jì)算量，延長(zhǎng)了誤差估計(jì)和校正的時(shí)間。因此，基于MEMSMU的行人導(dǎo) 航方法有二個(gè)關(guān)鍵問(wèn)題亟需解決：
[0008] 1、引入后向平滑技術(shù)進(jìn)行準(zhǔn)實(shí)時(shí)的誤差估計(jì)；
[0009] 2、簡(jiǎn)化濾波平滑模型匹配傳感器的采樣頻率；
[0010] 3、劃分步態(tài)時(shí)相確定濾波平滑算法的執(zhí)行區(qū)間。
[0011] 本發(fā)明提供的基于雙向?yàn)V波平滑技術(shù)的步進(jìn)式行人導(dǎo)航方法綜合利用了捷聯(lián)慣 導(dǎo)算法、零速校正算法、卡爾曼濾波算法和固定區(qū)間平滑算法，采用前饋加反饋的復(fù)合控制 策略和基于雙向?yàn)V波平滑技術(shù)的誤差估計(jì)方法，具有前向、后向、再前向三個(gè)步進(jìn)式算法通 道，構(gòu)成了 一個(gè)包含開環(huán)控制和閉環(huán)控制的復(fù)合控制系統(tǒng)。
[0012] 本發(fā)明提供了一種基于雙向?yàn)V波平滑技術(shù)的步進(jìn)式行人導(dǎo)航方法，所述方法包 括：
[0013] 步驟101，采集人體行走數(shù)據(jù)，其中，所述人體行走數(shù)據(jù)包括人體行走時(shí)的三維加 速度數(shù)據(jù)和三維角速度數(shù)據(jù)；
[0014] 步驟102,在第一前向算法通道中，先對(duì)采集到的人體行走數(shù)據(jù)進(jìn)行誤差校正，再 對(duì)校正后的人體行走數(shù)據(jù)進(jìn)行捷聯(lián)慣導(dǎo)解算，獲得人體行走時(shí)初步的姿態(tài)信息和速度信 息；同時(shí)進(jìn)行步態(tài)時(shí)相的檢測(cè)和步進(jìn)區(qū)間的劃分，當(dāng)檢測(cè)到人體足部處于步態(tài)周期的支撐 相時(shí)，在誤差域進(jìn)行姿態(tài)誤差和速度誤差的開環(huán)卡爾曼濾波估計(jì)；當(dāng)劃分出一個(gè)新的步進(jìn) 區(qū)間時(shí)，第一前向算法通道暫停，運(yùn)行后向算法通道和第二前向算法通道；
[0015] 步驟103,在后向算法通道中，對(duì)第一前向算法通道中估計(jì)出的姿態(tài)誤差和速度誤 差進(jìn)行平滑處理，得到經(jīng)過(guò)平滑處理后的誤差估計(jì)；
[0016] 步驟104,在第二前向算法通道中，利用后向算法通道中平滑處理后的誤差估計(jì)對(duì) 第一前向算法通道中解算出的姿態(tài)信息和速度信息進(jìn)行誤差校正，再對(duì)誤差校正后的速度 信息進(jìn)行積分，解算出人體行走時(shí)的位置信息，并返回步驟102繼續(xù)運(yùn)行第一前向算法通 道。
[0017] 優(yōu)選的，通過(guò)固聯(lián)于人體足部的慣性傳感器來(lái)采集人體行走數(shù)據(jù)，通過(guò)便攜式導(dǎo) 航上位機(jī)來(lái)存儲(chǔ)和處理慣性傳感器采集的人體行走數(shù)據(jù)。
[0018] 優(yōu)選的，所述慣性傳感器為微慣性傳感器，所述微慣性傳感器在封裝時(shí)集成了一 個(gè)三軸加速度計(jì)和一個(gè)三軸陀螺儀。
[0019] 優(yōu)選的，在第一前向算法通道中，所述的先對(duì)采集到的人體行走數(shù)據(jù)進(jìn)行誤差校 正，再對(duì)校正后的人體行走數(shù)據(jù)進(jìn)行捷聯(lián)慣導(dǎo)解算，包括：
[0020] 利用以下公式進(jìn)行人體行走數(shù)據(jù)的誤差校正，得到誤差校正后的人體行走數(shù)據(jù)：
[0021]
【主權(quán)項(xiàng)】
1. 一種基于雙向?yàn)V波平滑技術(shù)的步進(jìn)式行人導(dǎo)航方法，其特征在于，所述行人導(dǎo)航方 法包括： 步驟101，采集人體行走數(shù)據(jù)，其中，所述人體行走數(shù)據(jù)包括人體行走時(shí)的三維加速度 數(shù)據(jù)和三維角速度數(shù)據(jù)； 步驟102,在第一前向算法通道中，先對(duì)采集到的人體行走數(shù)據(jù)進(jìn)行誤差校正，再對(duì)校 正后的人體行走數(shù)據(jù)進(jìn)行捷聯(lián)慣導(dǎo)解算，獲得人體行走時(shí)初步的姿態(tài)信息和速度信息；同 時(shí)進(jìn)行步態(tài)時(shí)相的檢測(cè)和步進(jìn)區(qū)間的劃分，當(dāng)檢測(cè)到人體足部處于步態(tài)周期的支撐相時(shí)， 在誤差域進(jìn)行姿態(tài)誤差和速度誤差的開環(huán)卡爾