基于人體/環(huán)境約束的行人導(dǎo)航系統(tǒng)三維空間定位方法
【專利摘要】本發(fā)明涉及一種基于人體/環(huán)境約束的行人導(dǎo)航系統(tǒng)三維空間定位方法，解決了基于低成本MEMS慣性測(cè)量單元設(shè)計(jì)的行人導(dǎo)航系統(tǒng)在三維空間內(nèi)的高精度定位問題；本方法采用鞋部安裝MEMS慣性測(cè)量單元的方式進(jìn)行數(shù)據(jù)采集并完成捷聯(lián)導(dǎo)航解算，同時(shí)結(jié)合鞋底安裝的壓力傳感器輸出的數(shù)據(jù)對(duì)行人步態(tài)中的零速區(qū)間進(jìn)行檢測(cè)。本發(fā)明結(jié)合行人運(yùn)動(dòng)特點(diǎn)及周圍環(huán)境中建筑物特點(diǎn)，以零速修正為基礎(chǔ)，提出了抑制零速區(qū)間內(nèi)磁場(chǎng)異常及抑制SINS高度通道發(fā)散的方案，并設(shè)計(jì)了擴(kuò)展卡爾曼濾波器對(duì)導(dǎo)航解算結(jié)果進(jìn)行誤差修正，實(shí)現(xiàn)了行人在三維空間的高精度定位功能。
【專利說明】
基于人體/環(huán)境約束的行人導(dǎo)航系統(tǒng)三維空間定位方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明屬于導(dǎo)航定位技術(shù)領(lǐng)域，具體來說，涉及一種基于人體/環(huán)境約束的行人導(dǎo) 航系統(tǒng)三維空間定位方法。
【背景技術(shù)】
[0002] 行人導(dǎo)航系統(tǒng)能夠幫助執(zhí)行任務(wù)的人員實(shí)時(shí)定位自己的位置并與指揮中心取得 聯(lián)系，從而極大地保障緊急救援人員在未知環(huán)境中的生命安全。此外，在機(jī)場(chǎng)、劇院、地下停 車場(chǎng)等大型公共場(chǎng)所及高樓林立的現(xiàn)代化城市中，精度高、實(shí)時(shí)性強(qiáng)的行人導(dǎo)航系統(tǒng)能夠 有效地幫助行人進(jìn)行自我定位及目標(biāo)找尋。但現(xiàn)有的衛(wèi)星定位系統(tǒng)在城市峽谷、室內(nèi)等衛(wèi) 星信號(hào)受限或者失效的環(huán)境中無法正常工作，因而無法實(shí)現(xiàn)連續(xù)自主定位。因此，開展無衛(wèi) 星信號(hào)下的行人導(dǎo)航定位技術(shù)具有廣泛的應(yīng)用前景。
[0003] 隨著MEMS技術(shù)的發(fā)展，MEMS傳感器具有成本低、重量輕、尺寸小、功耗低等優(yōu)勢(shì)，同 時(shí)考慮到慣性導(dǎo)航具有自主性強(qiáng)、抗干擾性強(qiáng)的特點(diǎn)，腳部安裝MEMS慣性傳感器的行人導(dǎo) 航系統(tǒng)成為了當(dāng)前的研究熱點(diǎn)。但MEMS傳感器一般精度較低，在采用SINS算法計(jì)算導(dǎo)航參 數(shù)時(shí)，積分過程中加速度誤差引起的位置誤差會(huì)隨時(shí)間的三次方增長。
[0004] 因此，如何提高定位的精度，已成為現(xiàn)在急需解決的問題。

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

[0005] 為解決上述問題，本發(fā)明提供了一種基于人體/環(huán)境約束的行人導(dǎo)航系統(tǒng)三維空 間定位方法，該方法包括如下步驟：
[0006] S101，在鞋跟部位安裝MEMS慣性測(cè)量單元，在鞋底腳跟部位安裝壓力傳感器；
[0007] S102,采集MEMS慣性測(cè)量單元及壓力傳感器輸出數(shù)據(jù)，提取行人運(yùn)動(dòng)步態(tài)中的零 速區(qū)間；具體提取行人運(yùn)動(dòng)步態(tài)中的零速區(qū)間的公式為：
[0008] amin<ax(k) 2+ay(k)2+az(k)2<a max
[0010] f(k)-Tf>0
[0011] Qy(k) I <Τω
[0012] 若以上四個(gè)判斷條件同時(shí)成立，則認(rèn)為當(dāng)前時(shí)刻行人腳部處于靜止?fàn)顟B(tài)；
[00?3 ] 其中，ai (k)表示k時(shí)刻i軸向上加速度計(jì)輸出，i = X，y，ζ。amin、amax分別是閾值區(qū)間 的左邊界和右邊界;ak是加速度計(jì)輸出，昆是窗口內(nèi)數(shù)據(jù)點(diǎn)的平均值，η表示窗口寬度， 為方差閾值;f(k)表示k時(shí)刻檢測(cè)到的壓力值，Tf表示壓力閾值；| coy(k)|表示k時(shí)刻y軸陀螺 輸出的絕對(duì)值，Τω為角速率閾值；
[0014] S103,利用步驟S102中MEMS慣性測(cè)量單元測(cè)量的加速度、角速度信息，并結(jié)合以下 公式進(jìn)行捷聯(lián)導(dǎo)航解算，得到行人的位置、速度、姿態(tài)信息；
[0018] 其中，上標(biāo)η表示導(dǎo)航坐標(biāo)系，b表示載體坐標(biāo)系。Q表示姿態(tài)四元數(shù)，V表示速度，P 表示位置。? 1是由b系到η系的傳輸矩陣，表示由陀螺儀測(cè)量的角速度構(gòu)成的反對(duì)稱矩 陣，fb表示比力，表示地球自轉(zhuǎn)角速度，ω二表示導(dǎo)航系相對(duì)地球系的轉(zhuǎn)動(dòng)角速率，g n表示 地球重力場(chǎng)矢量，在地球上某一范圍內(nèi)可近似為常值；
[0019] S104,利用步驟S102中提取到的零速區(qū)間開展卡爾曼濾波，估計(jì)行人系統(tǒng)狀態(tài)誤 差并對(duì)SINS解算結(jié)果進(jìn)行修正；
[0020] 系統(tǒng)卡爾曼濾波模型為：
[0021] 5xk+i= ?k5xk+wk
[0022] 5zk = Hk5xk+vk
[0023] 其中，Sxl^k時(shí)刻的系統(tǒng)狀態(tài)，是狀態(tài)轉(zhuǎn)移矩陣，概表示過程噪聲，SzAk時(shí)刻 誤差觀測(cè)量，Hk為測(cè)量矩陣，vk為測(cè)量噪聲；
[0024]定義誤差狀態(tài)向量為：
[0025] δχ=[δφη 5Vn δΡηeb Vb]
[0026] 其中誤差狀態(tài)向量中包含9個(gè)導(dǎo)航誤差及6個(gè)傳感器誤差，它們分別是三維的位置 誤差向量δ Φη，速度誤差向量δνη，姿態(tài)向量δΡη，陀螺偏置誤差向量J及加表偏置誤差向量 ▽ b;
[0027] 狀態(tài)轉(zhuǎn)移矩陣為：
[0029] 其中，At表示采樣時(shí)間間隔，fnX為導(dǎo)航坐標(biāo)系下比力分量構(gòu)成的反對(duì)稱矩陣；
[0030] 零速區(qū)間內(nèi)速度測(cè)量誤差表達(dá)式為：
[0031] AV = ViNS-Vst ance
[0032] 零速修正的量測(cè)矩陣為：
[0033] Hzupt=[03X3 13X3 03X3 03X3 03X3]
[0034] S105,在執(zhí)行步驟S104的同時(shí)，利用磁傳感器的輸出計(jì)算零速區(qū)間內(nèi)每一個(gè)零速 點(diǎn)對(duì)應(yīng)的航向角，在步驟S104執(zhí)行到零速區(qū)間內(nèi)最后一個(gè)零速點(diǎn)時(shí)，對(duì)零速區(qū)間內(nèi)磁傳感 器計(jì)算的所有航向角作平滑濾波，并將處理結(jié)果作為觀測(cè)量輸出給卡爾曼濾波器進(jìn)行航向 修正；
[0035] S106,在執(zhí)行步驟S104的同時(shí)，判斷環(huán)境約束條件下行人的高度變化特性，實(shí)時(shí)更 新高度觀測(cè)量并將其輸出給卡爾曼濾波器進(jìn)行高度修正；
[0036] 通過步驟S102到S106的循環(huán)執(zhí)行即可實(shí)時(shí)得到修正后的行人導(dǎo)航參數(shù)。
[0037] 進(jìn)一步的，上述MEMS慣性測(cè)量單元，包含三軸MEMS加速度計(jì)、三軸MEMS陀螺儀及三 軸MEMS磁傳感器。
[0038] 進(jìn)一步的，所述步驟S105中：在執(zhí)行步驟S104的同時(shí)，利用磁傳感器的輸出計(jì)算零 速區(qū)間內(nèi)每一個(gè)零速點(diǎn)對(duì)應(yīng)的航向角，在步驟S104執(zhí)行到零速區(qū)間內(nèi)最后一個(gè)零速點(diǎn)時(shí)， 對(duì)零速區(qū)間內(nèi)磁傳感器計(jì)算的所有航向角作平滑濾波，并將處理結(jié)果輸出給卡爾曼濾波 器;零速區(qū)間內(nèi)航向角及航向角測(cè)量誤差的計(jì)算表達(dá)式為： η-1
[0039] Headings = ^Heading,,^ (i) ?=1
[0040] Heading= (Headingt〇tai+HeadingMag(n) )/η [0041 ] Αφ = φΜ5 - Heading
[0042] 其中，η表示零速區(qū)間內(nèi)零速點(diǎn)的個(gè)數(shù)，Head i ngtotai表示零速區(qū)間內(nèi)除最后一個(gè)零 速點(diǎn)外其它零速點(diǎn)對(duì)應(yīng)的磁航向角總和，Head i ngMag (i)表示零速區(qū)間內(nèi)第i個(gè)零速點(diǎn)對(duì)應(yīng) 的磁航向角，Heading零速區(qū)間內(nèi)磁傳感器計(jì)算的所有航向角作平滑濾波的值，Δ爐表示航 向角誤差，表不SINS計(jì)算得到的航向角。
[0043]航向修正的量測(cè)矩陣為：
[0044] Hheading=[[0 0 1] 〇3X3 〇3X3 〇3X3 〇3X3]
[0045] 進(jìn)一步的，所述步驟S106中：在執(zhí)行步驟S104的同時(shí)，判斷在由環(huán)境約束條件下的 行人高度變化特性，實(shí)時(shí)更新高度觀測(cè)量并將其輸出給卡爾曼濾波器;利用樓梯上臺(tái)階高 度進(jìn)行約束時(shí)，需首先提取步態(tài)中的零速區(qū)間，然后判斷零速區(qū)間內(nèi)相鄰零速點(diǎn)之間高度 變化值與設(shè)定高度閾值間的關(guān)系，識(shí)別行人的運(yùn)動(dòng)狀態(tài);定義k時(shí)刻SINS解算的高度信息為 hlNS，k-l時(shí)刻更新后的高度信息為hprev，二者的高度差記為Ah = hiNS-hprev，高度閾值記為 thh，k時(shí)刻更新后的高度信息為h_r，此時(shí)導(dǎo)航解算的高度值與觀測(cè)的高度值間的關(guān)系可表 達(dá)為：
[0046] Δ H=hiNS-hcuur
[0047] 高度修正的量測(cè)矩陣為：
[0048] Hheight= [03X3 03X3 [0 0 l] 〇3X3 〇3X3]〇
[0049] 與現(xiàn)有技術(shù)相比，本發(fā)明所述的基于人體/環(huán)境約束的行人導(dǎo)航系統(tǒng)三維空間定 位方法具有如下優(yōu)點(diǎn)：
[0050] (1)本發(fā)明在利用MEMS慣性測(cè)量單元輸出數(shù)據(jù)進(jìn)行零速區(qū)間檢測(cè)的基礎(chǔ)上，引入 壓力傳感器輔助MIMU提取步態(tài)中的零速區(qū)間?；趬毫鞲衅鞯妮敵鎏匦?，多傳感器數(shù)據(jù) 輸出組合的方式極大地降低了零速區(qū)間的誤檢與漏檢率，能有效提高行人導(dǎo)航系統(tǒng)的解算 精度。
[0051] (2)本發(fā)明結(jié)合行人運(yùn)動(dòng)特點(diǎn)，對(duì)零速區(qū)間內(nèi)磁傳感器計(jì)算的航向角作平滑濾波， 降低了局部零速點(diǎn)處磁場(chǎng)異常導(dǎo)致的航向誤差，從而在保證航向修正的基礎(chǔ)上提高了可靠 性。
[0052] (3)本發(fā)明所述的方法考慮了 SINS解算過程中高度通道發(fā)散的問題，同時(shí)結(jié)合行 人運(yùn)動(dòng)過程中周圍環(huán)境中多層建筑物內(nèi)外存在樓梯且樓梯上臺(tái)階高度一致的特性，提出了 利用樓梯尺寸進(jìn)行高度約束的方法，提高了行人系統(tǒng)在三維空間內(nèi)的定位精度。
【附圖說明】
[0053]圖1為基于人體/環(huán)境約束的行人導(dǎo)航系統(tǒng)三維空間定位方法流程圖。
[0054] 圖2為基于零速區(qū)間內(nèi)航向角約束的導(dǎo)航解算流程圖。
[0055] 圖3為基于環(huán)境中樓梯臺(tái)階高度約束的導(dǎo)航解算流程圖。
【具體實(shí)施方式】
[0056] 下面通過具體實(shí)施例對(duì)本發(fā)明作進(jìn)一步詳述，以下實(shí)施例只是描述性的，不是限 定性的，不能以此限定本發(fā)明的保護(hù)范圍。
[0057]如圖1所示，本發(fā)明請(qǐng)求保護(hù)一種基于人體/環(huán)境約束的行人導(dǎo)航系統(tǒng)三維空間定 位方法，該方法包含以下步驟：
[0058] S101，在鞋跟部位安裝MEMS(Micr〇-Electro_Mechanical System)慣性測(cè)量單元， Mmu坐標(biāo)軸分別指向人體的前、右、下方向；在鞋底腳跟部位安裝壓力傳感器，保證其敏感 軸朝豎直方向；
[0059] 上述MEMS慣性測(cè)量單元，包含三軸MEMS加速度計(jì)、三軸MEMS陀螺儀及三軸MEMS磁 傳感器；
[0060] S102,采集MEMS慣性測(cè)量單元及壓力傳感器輸出數(shù)據(jù)，提取行人運(yùn)動(dòng)步態(tài)中的零 速區(qū)間；具體提取行人運(yùn)動(dòng)步態(tài)中的零速區(qū)間的公式為：
[0061 ] ￡lmin<￡lx(k) +￡ly(k) +￡lz(k) <￡lmax
[0063] f(k)-Tf>0
[0064] oy(k) | <Τω
[0065] 若以上四個(gè)判斷條件同時(shí)成立，則認(rèn)為當(dāng)前時(shí)刻行人腳部處于靜止?fàn)顟B(tài)；
[0066] 其中，ai (k)表示k時(shí)亥lj i軸向上加速度計(jì)輸出，i = X，y，ζ。amin、ama^別是閾值區(qū)間 的左邊界和右邊界；ak是加速度計(jì)輸出，&是窗口內(nèi)數(shù)據(jù)點(diǎn)的平均值，η表示窗口寬度， 為方差閾值;f(k)表示k時(shí)刻檢測(cè)到的壓力值，Tf表示壓力閾值；| coy(k)|表示k時(shí)刻y 軸陀螺輸出的絕對(duì)值，Τω為角速率閾值；
[0067] S103,利用步驟S102中MEMS慣性測(cè)量單元測(cè)量的加速度、角速度信息，并結(jié)合以下 公式進(jìn)行捷聯(lián)導(dǎo)航解算，得到行人的位置、速度、姿態(tài)信息；
[0069] r! = Clfb -(2αζ + <)xV"+ gn
[0070] pn - γη
[0071] 其中，上標(biāo)n表示導(dǎo)航坐標(biāo)系，b表示載體坐標(biāo)系。Q表示姿態(tài)四元數(shù)，V表示速度，P 表示位置。^是由b系到η系的傳輸矩陣，6&X表示由陀螺儀測(cè)量的角速度構(gòu)成的反對(duì)稱 矩陣，fb表示比力，<2《表示地球自轉(zhuǎn)角速度，表示導(dǎo)航系相對(duì)地球系的轉(zhuǎn)動(dòng)角速率，gn表 示地球重力場(chǎng)矢量，在地球上某一范圍內(nèi)可近似為常值。
[0072] S104,利用步驟S102中提取到的零速區(qū)間開展卡爾曼濾波，估計(jì)行人系統(tǒng)狀態(tài)誤 差并對(duì)SINS解算結(jié)果進(jìn)行修正；
[0073]系統(tǒng)卡爾曼濾波模型為：
[0074] 5xk+i= ?k5xk+Wk
[0075] 5zk = Hk5xk+vk
[0076] 其中，3處是1^時(shí)刻的系統(tǒng)狀態(tài)，是狀態(tài)轉(zhuǎn)移矩陣，概表示過程噪聲，SZkSk時(shí)刻 誤差觀測(cè)量，H k為測(cè)量矩陣，vk為測(cè)量噪聲；
[0077]定義誤差狀態(tài)向量為：
[0078] δχ=[δφη 5Vn δΡηeb Vb]
[0079] 其中誤差狀態(tài)向量中包含9個(gè)導(dǎo)航誤差及6個(gè)傳感器誤差，它們分別是三維的位置 誤差向量δ Φη，速度誤差向量δνη，姿態(tài)向量δρη，陀螺偏置誤差向量J及加表偏置誤差向量 Vb〇
[0080] 狀態(tài)轉(zhuǎn)移矩陣為：
[0082]其中，At表示采樣時(shí)間間隔，fnX為導(dǎo)航坐標(biāo)系下比力分量構(gòu)成的反對(duì)稱矩陣。 [0083] 零速區(qū)間內(nèi)速度測(cè)量誤差表達(dá)式為：
[0084] AV = ViNS-Vst ance
[0085] 零速修正的量測(cè)矩陣為：
[0086] HZUpt=[03X3 13X3 03X3 03X3 03X3]
[0087] S105,在執(zhí)行步驟S104的同時(shí)，利用磁傳感器的輸出計(jì)算零速區(qū)間內(nèi)每一個(gè)零速 點(diǎn)對(duì)應(yīng)的航向角，在步驟S104執(zhí)行到零速區(qū)間內(nèi)最后一個(gè)零速點(diǎn)時(shí)，對(duì)零速區(qū)間內(nèi)磁傳感 器計(jì)算的所有航向角作平滑濾波，并將處理結(jié)果輸出給卡爾曼濾波器進(jìn)行航向修正；
[0088]圖2為基于零速區(qū)間內(nèi)航向角約束的導(dǎo)航解算流程圖，零速區(qū)間內(nèi)航向角及航向 角測(cè)量誤差的計(jì)算表達(dá)式為： n-l
[0089] Headingtml =^Heading"uv (/) .Μ.
[0090] Heading= (Headingt〇tai+HeadingMag(n) )/n
[0091 ] Αφ = - Heading
[0092] 其中，n表示零速區(qū)間內(nèi)零速點(diǎn)的個(gè)數(shù)，Head i ngtotai表示零速區(qū)間內(nèi)除最后一個(gè)零 速點(diǎn)外其它零速點(diǎn)對(duì)應(yīng)的磁航向角總和，Head i ngMag (i)表示零速區(qū)間內(nèi)第i個(gè)零速點(diǎn)對(duì)應(yīng) 的磁航向角，Heading零速區(qū)間內(nèi)磁傳感器計(jì)算的所有航向角作平滑濾波的值，Δ0_表示航 向角誤差，約鄉(xiāng)表不31吧計(jì)算得到的航向角。
[0093]航向修正的量測(cè)矩陣為：
[0094] Hheading=[[0 0 1] 〇3Χ3 〇3Χ3 〇3Χ3 〇3Χ3]
[0095] S106,在執(zhí)行步驟S104的同時(shí)，判斷在由環(huán)境約束條件下的行人高度變化特性，實(shí) 時(shí)更新高度觀測(cè)量并將其輸出給卡爾曼濾波器進(jìn)行高度修正。
[0096] 圖3為基于環(huán)境中樓梯臺(tái)階高度約束的導(dǎo)航解算流程圖。利用樓梯上臺(tái)階高度進(jìn) 行約束時(shí)，需首先提取步態(tài)中的零速區(qū)間，然后判斷零速區(qū)間內(nèi)相鄰零速點(diǎn)之間高度變化 值與設(shè)定高度閾值間的關(guān)系，識(shí)別行人的運(yùn)動(dòng)狀態(tài)(上樓、下樓、水平運(yùn)動(dòng)）。定義k時(shí)刻SINS 解算的高度信息為h INS，k_l時(shí)刻更新后的高度信息為hprev，二者的高度差記為Ah = hINS_ hpre3V，高度閾值記為thh，k時(shí)刻更新后的高度信息為h_ r，此時(shí)導(dǎo)航解算的高度值與觀測(cè)的 高度值間的關(guān)系可表達(dá)為：
[0097] Δ H=hiNS-hcuur
[0098] 高度修正的量測(cè)矩陣為：
[0099] Hheight=[03X3 03Χ3 [0 0 1] 〇3X3 〇3X3]
[0100] 通過步驟S102到S106的循環(huán)執(zhí)行即可實(shí)時(shí)得到修正后的行人導(dǎo)航參數(shù)。
[0101] 應(yīng)當(dāng)理解的是，本發(fā)明的應(yīng)用不限于上述的舉例，對(duì)本領(lǐng)域普通技術(shù)人員來說，可 以根據(jù)上述說明加以改進(jìn)或變換，所有這些改進(jìn)和變換都應(yīng)屬于本發(fā)明所附權(quán)利要求的保 護(hù)范圍。
【主權(quán)項(xiàng)】
1. 一種基于人體/環(huán)境約束的行人導(dǎo)航系統(tǒng)三維空間定位方法，其特征在于，該方法包 括如下步驟： SlOl，在鞋跟部位安裝MEMS慣性測(cè)量單元，在鞋底腳跟部位安裝壓力傳感器； S102,采集MEMS慣性測(cè)量單元及壓力傳感器輸出數(shù)據(jù)，提取行人運(yùn)動(dòng)步態(tài)中的零速區(qū) 間；具體提取行人運(yùn)動(dòng)步態(tài)中的零速區(qū)間的公式為：若以上四個(gè)判斷條件同時(shí)成立，則認(rèn)為當(dāng)前時(shí)刻行人腳部處于靜止?fàn)顟B(tài)； 其中，ai (k)表示k時(shí)刻i軸向上加速度計(jì)輸出，i = X，y，z。amin、amax分別是閾值區(qū)間的左 邊界和右邊界;￡ik是加速度計(jì)輸出，<是窗口內(nèi)數(shù)據(jù)點(diǎn)的平均值，η表示窗口寬度，L ,為 方差閾值;f(k)表示k時(shí)刻檢測(cè)到的壓力值，Tf表示壓力閾值；I coy(k) I表示k時(shí)刻y軸陀螺輸 出的絕對(duì)值，Τω為角速率閾值； S103，利用步驟S102中MEMS慣性測(cè)量單元測(cè)量的加速度、角速度信息，并結(jié)合以下公式 進(jìn)行捷聯(lián)導(dǎo)航解算，得到行人的位置、速度、姿態(tài)信息；其中，上標(biāo)η表示導(dǎo)航坐標(biāo)系，b表示載體坐標(biāo)系。Q表示姿態(tài)四元數(shù)，V表示速度，P表示 位置。Cf是由b系到η系的傳輸矩陣，X表示由陀螺儀測(cè)量的角速度構(gòu)成的反對(duì)稱矩陣， fb表示比力，<表示地球自轉(zhuǎn)角速度，份=,表示導(dǎo)航系相對(duì)地球系的轉(zhuǎn)動(dòng)角速率，gn表示地 球重力場(chǎng)矢量，在地球上某一范圍內(nèi)可近似為常值； S104,利用步驟S102中提取到的零速區(qū)間開展卡爾曼濾波，估計(jì)行人系統(tǒng)狀態(tài)誤差并 對(duì)SINS解算結(jié)果進(jìn)行修正； 系統(tǒng)卡爾曼濾波模型為： 3xk+l = Φ k3xk+Wk 3zk - HkSxk+Vk 其中，3^是1^時(shí)刻的系統(tǒng)狀態(tài)，是狀態(tài)轉(zhuǎn)移矩陣，概表示過程噪聲，SzkSk時(shí)刻誤差觀 測(cè)量，Hk為測(cè)量矩陣，Vk為測(cè)量噪聲； 定義誤差狀態(tài)向量為： δχ=[δφη δνη δρηeb vb] 其中誤差狀態(tài)向量中包含9個(gè)導(dǎo)航誤差及6個(gè)傳感器誤差，它們分別是三維的位置誤差 向量δ Φη，速度誤差向量δνη，姿態(tài)向量δΡη，陀螺偏置誤差向量J及加表偏置誤差向量V b; 狀態(tài)轉(zhuǎn)移矩陣為：其中，At表示采樣時(shí)間間隔，fnX為導(dǎo)航坐標(biāo)系下比力分量構(gòu)成的反對(duì)稱矩陣； 零速區(qū)間內(nèi)速度測(cè)量誤差表達(dá)式為： AV = ViNs-Vst ance 零速修正的量測(cè)矩陣為： Hzupt= [〇3Χ3 13X3 〇3Χ3 〇3Χ3 〇3Χ3] 5105, 在執(zhí)行步驟S104的同時(shí)，利用磁傳感器的輸出計(jì)算零速區(qū)間內(nèi)每一個(gè)零速點(diǎn)對(duì) 應(yīng)的航向角，在步驟S104執(zhí)行到零速區(qū)間內(nèi)最后一個(gè)零速點(diǎn)時(shí)，對(duì)零速區(qū)間內(nèi)磁傳感器計(jì) 算的所有航向角作平滑濾波，并將處理結(jié)果作為觀測(cè)量輸出給卡爾曼濾波器進(jìn)行航向修 正； 5106, 在執(zhí)行步驟S104的同時(shí)，判斷環(huán)境約束條件下行人的高度變化特性，實(shí)時(shí)更新高 度觀測(cè)量并將其輸出給卡爾曼濾波器進(jìn)行高度修正； 通過步驟S102到S106的循環(huán)執(zhí)行即可實(shí)時(shí)得到修正后的行人導(dǎo)航參數(shù)。2. 如權(quán)利要求1所述的方法，其特征在于，上述MEMS慣性測(cè)量單元，包含三軸MEMS加速 度計(jì)、三軸MEMS陀螺儀及三軸MEMS磁傳感器。3. 如權(quán)利要求1所述的方法，其特征在于： 所述步驟S105中：在執(zhí)行步驟S104的同時(shí)，利用磁傳感器的輸出計(jì)算零速區(qū)間內(nèi)每一 個(gè)零速點(diǎn)對(duì)應(yīng)的航向角，在步驟S104執(zhí)行到零速區(qū)間內(nèi)最后一個(gè)零速點(diǎn)時(shí)，對(duì)零速區(qū)間內(nèi) 磁傳感器計(jì)算的所有航向角作平滑濾波，并將處理結(jié)果輸出給卡爾曼濾波器;零速區(qū)間內(nèi) 航向角及航向角測(cè)量誤差的計(jì)算表達(dá)式為：其中，η表示零速區(qū)間內(nèi)零速點(diǎn)的個(gè)數(shù)，HeadingtoteI表示零速區(qū)間內(nèi)除最后一個(gè)零速點(diǎn) 外其它零速點(diǎn)對(duì)應(yīng)的磁航向角總和，Head i ngMag (i)表示零速區(qū)間內(nèi)第i個(gè)零速點(diǎn)對(duì)應(yīng)的磁 航向角，Heading零速區(qū)間內(nèi)磁傳感器計(jì)算的所有航向角作平滑濾波的值，Δ妒表示航向角 誤差，表不SINS計(jì)算得到的航向角。 航向修正的量測(cè)矩陣為： Hheading一 [ [O O I ] 〇3X3 〇3X3 〇3X3 〇3X3 ] 〇4.如權(quán)利要求1所述的方法，其特征在于： 所述步驟S106中：在執(zhí)行步驟S104的同時(shí)，判斷在由環(huán)境約束條件下的行人高度變化 特性，實(shí)時(shí)更新高度觀測(cè)量并將其輸出給卡爾曼濾波器;利用樓梯上臺(tái)階高度進(jìn)行約束時(shí)， 需首先提取步態(tài)中的零速區(qū)間，然后判斷零速區(qū)間內(nèi)相鄰零速點(diǎn)之間高度變化值與設(shè)定高 度閾值間的關(guān)系，識(shí)別行人的運(yùn)動(dòng)狀態(tài);定義k時(shí)刻SINS解算的高度信息為h INS，k-l時(shí)刻更 新后的高度信息為hprev，二者的高度差記為Ah = hINS-hprev，高度閾值記為thh，k時(shí)刻更新后 的高度信息為h_ r，此時(shí)導(dǎo)航解算的高度值與觀測(cè)的高度值間的關(guān)系可表達(dá)為： A H=hiNS-h cuur 高度修正的量測(cè)矩陣為： Hheight= [〇3Χ3 〇3Χ3 [〇 O l] 〇3X3 〇3X3]〇
【文檔編號(hào)】G01C21/16GK106017461SQ201610339701
【公開日】2016年10月12日
【申請(qǐng)日】2016年5月19日
【發(fā)明人】田曉春, 韓勇強(qiáng), 陳家斌, 謝玲, 李楠
【申請(qǐng)人】北京理工大學(xué)