[發明專利]一種具有自主性的室內人員高程估計方法

有效

• 申請號: 202110766046.4
• 申請日: 2021-06-30
• 公開（公告）號: CN113483763B
• 公開（公告）日: 2022-11-18
• 發明（設計）人: 孫暢;夏鳴
• 申請（專利權）人: 北京航空航天大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/16
• 代理公司: 廣州晟策知識產權代理事務所(普通合伙) 44709
• 代理人: 鄭書鑫;李靖

1.一種具有自主性的室內人員高程估計方法，其特征在于，包括如下方法步驟：

步驟一、從安裝于足部的MEMS-IMU傳感器中采集三維加速度和三維角速度數據，并進行捷聯解算，得到姿態，速度和位置信息，如式(1)所示

其中，上標n和b分別表示導航坐標系和載體坐標系；Vⁿ和分別表示導航坐標系下的三維速度及其微分；表示導航坐標系下三維位置的微分；和分別表示由b系到n系轉換的姿態矩陣及其微分；Ω^b表示由陀螺儀輸出角速度構成的反對稱矩陣；f^b表示載體坐標系下的比力；gⁿ表示地球重力場矢量；

步驟二、根據加速度和角速度數據，計算零速更新的統計值，實現對行走中傳感器所在腳周期性觸地的階段進行檢測；

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

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

其中，Stn_m，i是第m個步周期的第i個采樣時刻，i取從1到步周期靜止態的采樣數目last之間的整數，對不同的步周期，last取值不相同；

步驟四、基于步驟三所計算的速度誤差作為觀測量，采用擴展卡爾曼濾波(EKF)對步驟一中所求的定位結果三維速度、三維位置、姿態進行修正；

定義EKF的15維誤差狀態向量如下：

其中，δPⁿ、δVⁿ、δφⁿ、ε^b和分別代表為三維位置誤差向量、三維速度誤差向量、三維姿態誤差向量、三維陀螺漂移和三維加速度偏置；

步驟五、計算步間高度增量Δh，如下所示：

Δh(m)＝h(m)-h(m-1)(4)

其中，m為步態周期序號，m＞1；h表示采用EKF修正后的垂直位移。

2.根據權利要求1所述的具有自主性的室內人員高程估計方法，還包括步驟六：對人員的水平行走運動狀態進行判斷，假設δ為判定閾值，取值為正，當第m步的高度增量的絕對值|Δh(m)|小于門限值δ時，為水平行走狀態；當第m步的高度增量的絕對值|Δh(m)|大于門限值δ時，為上下樓梯狀態。

3.根據權利要求2所述的具有自主性的室內人員高程估計方法，從初始時刻開始至當前步周期內，人員的運動狀態均為水平行走時，還包括步驟七，更新步間高程誤差均值

其中，m表示步態周期的序號；Δh為步間高度增量。

4.根據權利要求3所述的具有自主性的室內人員高程估計方法，從初始時刻開始至當前步周期內，人員的運動狀態均為上下樓梯狀態時，還包括步驟八，根據式(6)對EKF修正后的垂直位移Δh進行補償：

其中，為高程誤差的均值；m表示步態周期的序號；為補償后的步間高度差。

5.根據權利要求4所述的具有自主性的室內人員高程估計方法，根據判斷人員在垂直方向上的7種運動狀態{上一個臺階，上兩個臺階，上三個臺階，上四個臺階，平面行走，下一個臺階，下兩個臺階，下三個臺階，下四個臺階}，并分別用{+1，+2，+3，+4，0，-1，-2，-3，-4}表示：

Clss_m取值為0，±1，±2，±3，±4 (7)

其中，α表示單個樓梯臺階高度；Clss_m為步周期為m的人員的運動狀態類型；是步驟八中進行垂直誤差補償后的步間高度差；round表示取最近整數的函數。

6.根據權利要求5所述的具有自主性的室內人員高程估計方法，計算當前步周期靜止態Stn_m時的高度

其中，Clss_m為步周期為m的人員的運動狀態類型；σ表示單個樓梯臺階的高度；m為步態周期的序號。