[發(fā)明專利]一種具有自主性的室內(nèi)人員高程估計(jì)方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202110766046.4
• 申請(qǐng)日: 2021-06-30
• 公開（公告）號(hào): CN113483763B
• 公開（公告）日: 2022-11-18
• 發(fā)明（設(shè)計(jì)）人: 孫暢;夏鳴
• 申請(qǐng)（專利權(quán)）人: 北京航空航天大學(xué)
• 主分類號(hào): G01C21/20
• 分類號(hào): G01C21/20;G01C21/16
• 代理公司: 廣州晟策知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 44709
• 代理人: 鄭書鑫;李靖

本發(fā)明公開了一種具有自主性的室內(nèi)人員高程估計(jì)方法，基于鞋嵌式MEMS?IMU，不受外部氣象條件制約和電磁干擾，無(wú)需外部布設(shè)和前期信息勘測(cè)等工作，具有自主性；對(duì)步間高度增量進(jìn)行了補(bǔ)償后，再對(duì)垂直運(yùn)動(dòng)狀態(tài)進(jìn)行分類，可計(jì)算每步跨越不同臺(tái)階數(shù)時(shí)的垂直位移；算法復(fù)雜度低，易于工程實(shí)現(xiàn)。

技術(shù)領(lǐng)域

本發(fā)明涉及行室內(nèi)定位領(lǐng)域，特別涉及一種具有自主性的室內(nèi)人員高程估計(jì)方法。

背景技術(shù)

《2018年室內(nèi)定位的發(fā)展前景分析》報(bào)告指出，我國(guó)室內(nèi)定位直接市場(chǎng)總量已突破3000億元。目前，化工廠、醫(yī)院、養(yǎng)老院、監(jiān)獄/看守所/戒毒所、管廊施工、火場(chǎng)等正大規(guī)模引入室內(nèi)定位?，F(xiàn)實(shí)中，室內(nèi)運(yùn)動(dòng)軌跡不僅包括水平分量，還含有垂直位移。人員垂直位移對(duì)樓層判斷十分重要，同時(shí)面臨大的挑戰(zhàn)。

在室內(nèi)，由于墻壁遮擋和多徑效應(yīng)的影響，GNSS(Global Navigation SatelliteSystem,全球?qū)Ш叫l(wèi)星系統(tǒng))無(wú)法提供有效的高度位置信息。目前，大部分關(guān)于行人垂直定位的研究，多采用在不同樓層部署輔助信標(biāo)裝置，如WIFI、UWB(Ultra Wide Band,超寬帶)等，而基礎(chǔ)設(shè)施布設(shè)或指紋數(shù)據(jù)庫(kù)采集，耗費(fèi)大量人力物力，且在緊急應(yīng)用案例中，基礎(chǔ)設(shè)施不可用的情況，時(shí)有發(fā)生。另外，大氣壓強(qiáng)與高度呈一定規(guī)律變化，氣壓計(jì)也被用于高程位置估計(jì)，但是，氣壓受溫度、濕度、風(fēng)速的變化等因素影響。

慣性定位的核心部件是IMU(Inertial Measurement Unit,慣性測(cè)量單元)，不受外部電磁信號(hào)和氣象變化干擾，也無(wú)需外部布設(shè)和前期信息勘測(cè)，自主性好。隨著MEMS(Micro Electro Mechanical System,微機(jī)電系統(tǒng))技術(shù)的發(fā)展，MEMS-IMU具有尺寸小、成本低、重量輕等優(yōu)點(diǎn)，在水平定位方面得到了研究人員的廣泛關(guān)注。但是，由于慣性垂直通道發(fā)散，MEMS-IMU無(wú)法直接用于高程估計(jì)。

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

為解決現(xiàn)有技術(shù)中的上述問題，即為解決傳統(tǒng)慣性方法中MEMS-IMU無(wú)法直接用于高程估計(jì)的問題，本發(fā)明提出了一種具有自主性的室內(nèi)高程估計(jì)方法，該方法不受氣象條件制約和外界電磁信號(hào)干擾，也無(wú)需任何外部布設(shè)與基礎(chǔ)設(shè)施維護(hù)工作。

本發(fā)明提出的一種具有自主性的室內(nèi)人員高程估計(jì)方法，主要包括以下步驟：

步驟一、從安裝于足部的MEMS-IMU傳感器中采集三維加速度和三維角速度數(shù)據(jù)，并進(jìn)行捷聯(lián)解算，如式(1)所示

其中，上標(biāo)n和b分別表示導(dǎo)航坐標(biāo)系和載體坐標(biāo)系；Vⁿ和分別表示導(dǎo)航坐標(biāo)系下的三維速度及其微分；表示導(dǎo)航坐標(biāo)系下三維位置的微分；和分別表示由b系到n系轉(zhuǎn)換的姿態(tài)矩陣及其微分；Ω^b表示由陀螺儀輸出角速度構(gòu)成的反對(duì)稱矩陣；f^b表示載體坐標(biāo)系下的比力；gⁿ表示地球重力場(chǎng)矢量；

步驟二、根據(jù)加速度和角速度數(shù)據(jù)，計(jì)算零速更新的統(tǒng)計(jì)值，實(shí)現(xiàn)對(duì)行走中傳感器所在腳周期性觸地的階段進(jìn)行檢測(cè)；

步驟三、傳感器所在腳相鄰兩次觸地之間為一個(gè)步態(tài)周期，步態(tài)周期序號(hào)記為m，并將第m個(gè)步態(tài)周期中的觸地靜止態(tài)記作Stn_m，觸地期間，實(shí)際速度為零，而基于步驟一解算的三維速度非零，可計(jì)算速度誤差如下所示：

dV(Stn_m,i)＝[0 0 0] (2)

其中，Stn_m,i是第m個(gè)步周期的第i個(gè)采樣時(shí)刻，i取從1到步周期靜止態(tài)的采樣數(shù)目last之間的整數(shù)，對(duì)不同的步周期，last取值不相同；

步驟四、基于步驟三所計(jì)算的速度誤差作為觀測(cè)量，采用擴(kuò)展卡爾曼濾波(EKF)對(duì)步驟一中所求的定位結(jié)果三維速度、三維位置、姿態(tài)進(jìn)行修正；

定義EKF的15維誤差狀態(tài)向量如下：