[發明專利]具有非視距識別功能的室內行人定位方法

在審

• 申請號: 201910608959.6
• 申請日: 2019-07-08
• 公開（公告）號: CN110426040A
• 公開（公告）日: 2019-11-08
• 發明（設計）人: 李文廣;李建增;史云飛;李永科;胡永江;李愛華;褚麗娜;馬朝陽
• 申請（專利權）人: 中國人民解放軍陸軍工程大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/18;G01C21/16;G01C21/00;G01S5/00;G01S5/02;G01S5/06;H04W4/02;H04W4/029;H04W4/33;H04W64/00
• 代理公司: 河北東尚律師事務所 13124
• 代理人: 王文慶

1.一種具有非視距識別功能的室內行人定位方法，其特征在于，包括以下步驟：

步驟一：利用信號到達時間法對待測行人目標進行定位，得到任意時刻下待測行人的位置坐標；

步驟二：根據待測行人的位置坐標，由三角不等式定理判斷待測行人的當前狀態是處于視距狀態還是非視距狀態；若待測行人處于視距狀態，則繼續執行步驟一；若待測行人處于非視距狀態，則執行步驟三；

步驟三：若待測行人處于非視距狀態，則采用慣性測量儀和行人航位推算法聯合定位的方法，推算待測行人在上一時刻到當前時刻的位移量；

步驟四：由上一時刻待測行人的位置坐標、上一時刻到當前時刻的位移量和IMU陀螺儀輸出的方向信息，重新計算當前時刻下待測行人的位置坐標；

步驟五：重復以上步驟，計算得到所有時刻下的待測行人的定位信息，利用粒子濾波器對數據進行篩選，去掉異常值；

完成具有非視距識別功能的室內行人定位。

2.根據權利要求1所述的一種具有非視距識別功能的室內行人定位方法，其特征在于，步驟一具體包括以下步驟：

(101)設定任意時刻下待測行人的位置坐標為(x₀,y₀)，基站的位置坐標為(x_i,y_i)，待測行人到第i個基站的真實距離為d_i，1≤i≤n，n為基站總數，則有：

(102)采用以下公式計算得到待測行人位置坐標：

將方程組中所有的方程均減去第一個方程，得如下方程組：

將上述方程組改寫為矩陣形式，則有如下形式：

Hx＝b (5)

矩陣方程的最小方差解為：

x＝(H^TH)^-1H^Tb (6)

由此，計算得到任意時刻下待測行人的位置坐標。

3.根據權利要求1所述的一種具有非視距識別功能的室內行人定位方法，其特征在于，步驟二中根據待測行人的位置坐標，由三角不等式定理判斷待測行人的當前狀態是處于視距狀態還是非視距狀態，具體包括以下步驟：

(201)由步驟一得到待測行人在當前時刻t的位置坐標(x_t,y_t)、在t-1時刻的位置坐標為(x_t-1,y_t-1)以及在t-2時刻的位置坐標為(x_t-2,y_t-2)；

(202)根據加速度測量及待測行人在t-1和t-2時刻的位置，計算待測行人從t-1時刻移動到t時刻的距離ΔS；

式中，v_t-1是t-1時刻的速度，是t-2到t-1時刻的平均速度，S_t-1,t-2是t-2到t-1時刻的移動距離，a_t-1和a_t-2分別表示t-1時刻的加速度和t-2時刻的加速度，t為當前時刻；

(203)計算待測行人在t時刻與基站(x_q,y_q)之間的距離d_t，目標在t-1時刻與基站(x_q,y_q)之間的距離d_t-1，1≤q≤n，n為基站總數，計算公式如下：

(204)由三角不等式定理得以下關系：

ΔS+d_t-d_t-1＞0 (15)

ΔS+d_t-1-d_t＞0 (16)

d_t+d_t-1-ΔS＞0 (17)

ΔS-||d_t-d_t-1||＞0 (18)

由于在實際中存在測量誤差和估計誤差，將式(18)改寫為：

ΔS-||d_t-d_t-1||＝ΔS-||d_t+ε_i+b_i-d_t-1-Δμ_t-1||＞0 (19)

ε_i+b_i是t時刻的測量誤差，Δμ_t-1是前一次位置的估計誤差；

(205)若式(18)或式(19)成立，則待測行人處于視距狀態；否則，待測行人處于非視距狀態。

4.根據權利要求1所述的一種具有非視距識別功能的室內行人定位方法，其特征在于，步驟三具體包括以下步驟：

(301)根據待測行人運動特點，建立行人腰部運動模型；

(302)計算一個周期待測行人在行走中腰部在水平方向上的位移量S_x；

ΔY₁＝L₁(1-cosδ) (20)

ΔY₂＝L₂(1-cosδ) (21)

S_right＝2L₁sinδ (22)

S_left＝2L₂cosδ (23)

由以上式子可得：

則一個周期行走中腰部的位移量為：

S_x＝S_right+S_left (26)

在總的步數下腰部的位移量S_d為：

式中，S₀為水平方向的初始位移量，S_right和S_left分別表示待測行人在支撐腳為右腳和左腳時人體行進的位移量，將行人垂直站立時慣性測量單元IMU與腳底的距離設定為單擺擺長，其中L₁和L₂分別表示右腳和左腳單腳支撐時行進過程中倒置單擺的擺長；ΔY₁和ΔY₂分別表示支撐腳為右腳和左腳時垂直方向上的最大和最小位移量；δ表示行進過程中腿部向前邁出的最大角度，即在單腿和雙腿支撐之間劃分臨界點地時刻，支撐腿與垂直地面之間的夾角角度；

(303)計算一個周期待測行人在行走中腰部在垂直方向上的位移量S_y；

待測行人在每次行進步態中，設定IMU輸出的三軸加速度分別為a_{x_i}，a_{y_i}，a_{z_i}，三軸角速度分別為ω_{x_i}，ω_{y_i}，ω_{z_i}，其中i＝1,2,…,N，N為總的步數；行進過程中支撐腿在垂直方向上的夾角為θ_i，則有：

其中，θ₀是θ_i的初值，ΔT為IMU的采樣時間間隔，利用θ_i將其轉換到水平和垂直方向上，則有：

其中，a_xi和a_yi分別為行進過程中在水平和垂直方向上的加速度值，g為重力加速度；

設定待測行人最開始是靜止的，即θ₀＝0，對加速度進行積分得到垂直方向上的速度：

利用“零速修正ZUPT”修正原理，得v_y0為零，通過對IMU垂直方向上的速度進行進一步積分運算，得到其在垂直方向上的位移量：

其中，S_y0為ZUPT時刻在垂直方向上的初始位移，將其設置為零；

求每一步中以ZUPT時刻為基準，實時位移S_yi的最大值和最小值，即可求得支撐腳為右腳和左腳時垂直方向上的最大和最小位移量ΔY₁和ΔY₂；通過式(27)得到一段時間內行人的總位移。