[發(fā)明專利]一種融合PDR和先驗地圖的行人室內(nèi)定位方法

在審

• 申請?zhí)枺?/td>: 202011338426.X
• 申請日: 2020-11-25
• 公開（公告）號: CN112562077A
• 公開（公告）日: 2021-03-26
• 發(fā)明（設計）人: 李慧平;陳青山;周良榮
• 申請（專利權(quán)）人: 西北工業(yè)大學
• 主分類號: G06T17/05
• 分類號: G06T17/05;G06T7/73;G06N3/00;G01C21/20;G01C21/18;G01C21/16;G01C21/14;G01C21/00
• 代理公司: 西北工業(yè)大學專利中心 61204
• 代理人: 金鳳

1.一種融合PDR和先驗地圖的行人室內(nèi)定位方法，其特征在于，包括以下步驟：

步驟1：繪制室內(nèi)地圖；

步驟2：對IMU進行標定處理，使用IMU采集行人室內(nèi)運動數(shù)據(jù)；

步驟3：使用IMU數(shù)據(jù)，利用PDR定位方法得到行人室內(nèi)軌跡；

步驟4：對行人室內(nèi)軌跡進行平滑處理；

步驟5：對平滑處理后的行人室內(nèi)軌跡進行旋轉(zhuǎn)和平移，與室內(nèi)地圖對準；

步驟6：對室內(nèi)地圖進行LSD線特征提取，把室內(nèi)地圖轉(zhuǎn)化為具有墻壁特征的地圖向量矩陣；

步驟7：將步驟5與室內(nèi)地圖對準后的行人室內(nèi)軌跡轉(zhuǎn)換成軌跡向量矩陣；使用LSI算法對地圖向量矩陣和軌跡向量矩陣進行判斷，得到行人室內(nèi)軌跡與墻壁的交叉點，即穿墻點；

步驟8：采用粒子群算法，對步驟5與室內(nèi)地圖對準后的行人室內(nèi)軌跡進行粒子濾波；

步驟9：在粒子濾波過程中，使用LSI算法檢測粒子群中是否有穿墻的粒子，并對穿墻的粒子進行權(quán)重歸零，得到地圖信息修正后的粒子群；

步驟10：粒子濾波完成后，計算出粒子群所表示的最優(yōu)值，得到最優(yōu)坐標，即最優(yōu)行人室內(nèi)軌跡。

2.根據(jù)權(quán)利要求1所述的一種融合PDR和先驗地圖的行人室內(nèi)定位方法，其特征在于，所述步驟1中繪制室內(nèi)地圖采用ARCGIS進行繪制。

3.根據(jù)權(quán)利要求1所述的一種融合PDR和先驗地圖的行人室內(nèi)定位方法，其特征在于，所述步驟2中對IMU進行標定處理的方法為：將IMU器件放入二十一面體中，然后把每個平面靜止十秒，利用采集到的數(shù)據(jù)對IMU進行標定處理。

4.根據(jù)權(quán)利要求1所述的一種融合PDR和先驗地圖的行人室內(nèi)定位方法，其特征在于，所述步驟3中使用IMU數(shù)據(jù)，利用PDR定位方法得到原始的行人室內(nèi)軌跡，包括如下步驟：

步驟3-1：MSINS初始對準：確定初始捷聯(lián)矩陣；

步驟3-2：MSINS捷聯(lián)解算；

進行四元數(shù)更新，即求解下式四元數(shù)微分方程：

其中，n表示導航坐標系，矩陣表示載體坐標系相對于n系的姿態(tài)四元數(shù)，定義載體坐標系為b系，是姿態(tài)角速率；

使用等效旋轉(zhuǎn)矢量法求解(1)式，通過旋轉(zhuǎn)矩陣鏈乘規(guī)則將式(1)寫成數(shù)字遞推的形式，如式(2)所示：

其中，是t_k時刻從b系到n系的變換四元數(shù)，是t_k-1時刻的姿態(tài)四元數(shù)；是以地心慣性坐標系為參考系時，n系從t_k-1時刻到t_k時刻的變換四元數(shù)，定義地心慣性坐標系為i系；是以i系為參考系時，b系從t_k-1時刻到t_k時刻的變換四元數(shù)；

計算n系從t_k-1時刻到t_k時刻的轉(zhuǎn)動等效旋轉(zhuǎn)矢量：

其中，是t時刻在導航坐標系下的角速度，是k-1時刻在導航坐標系下的角速度，是k時刻在導航坐標系下的角速度，τ_k＝t_k-t_k-1是采樣間隔；

根據(jù)等效旋轉(zhuǎn)與旋轉(zhuǎn)四元數(shù)之間的轉(zhuǎn)換關(guān)系，得到：

記b系從t_k-1時刻到t_k時刻的轉(zhuǎn)動等效旋轉(zhuǎn)矢量為采用角增量二子樣等效旋轉(zhuǎn)算法進行求解：

Δθ_k＝Δθ_k，1+Δθ_k，2 (6)其中，是陀螺儀測量得到的載體角速度，是一個快變量；Δθ_k是陀螺儀在t_k-1時刻到t_k時刻的角增量；Δθ_k，1和Δθ_k，2分別對應于陀螺儀從t_k-1時刻到時刻和從時刻到t_k時刻的角增量，計算方法為：

其中是t_k-1時刻的載體角速度，是時刻的載體角速度，是t_k時刻的載體角速度；

采用高速率采樣，使用單子樣算法求解計算方法為：

其中是單子樣方法下t_k-1時刻的載體角速度，是單子樣方法下t_k時刻的載體角速度；

根據(jù)等效旋轉(zhuǎn)矢量與旋轉(zhuǎn)四元數(shù)之間的轉(zhuǎn)換關(guān)系，得到：

將式(4)和式(10)代入到(1)式中，即完成姿態(tài)更新過程；

如果和有關(guān)的為[1，0，0，0]^T，則式(1)簡化為：

對式(9)進行簡化，得到：

步驟3-3：行人步態(tài)檢測和靜止檢測；

對于陀螺儀向量序列和加速度計向量序列構(gòu)造如下統(tǒng)計量：

式中，是陀螺儀在x軸的M個序列的平方和，是陀螺儀在y軸的M個序列的平方和，是陀螺儀在z軸的M個序列的平方和；是加速度計在x軸的M個序列的平方和，是加速度計在y軸的M個序列的平方和，是加速度計在z軸的M個序列的平方和；和分別是陀螺儀和加速度計向量的二范數(shù)平方；是陀螺儀序列的均值，是加速度計序列的均值；

由均值不等式，存在以下不等式的成立：

式中，分別是陀螺儀x軸、y軸、z軸采樣角速率序列{ω_ix}、{ω_iy}、{ω_iz}的均值；分別是加速度計x軸、y軸、z軸采樣比力序列{f_ix}、{f_iy}、{f_iz}的均值；上面不等式等號成立的條件是各測量軸的采樣序列元素全部相等；

式(16)表明，當序列各元素的離差越大，不等式兩端的差值越大；

對于陀螺儀，當其處于靜態(tài)時，三個測量軸輸出幾乎為零，可以直接利用式(16)兩端之差構(gòu)造反映角運動動態(tài)程度的特征量，如式(18)：

對于加速度計，加速度計輸出的比力數(shù)據(jù)含有重力加速度g分量，先從合矢量中把g扣除，按式(19)方法進行處理：

再將式(19)右側(cè)的替換成式(17)的右側(cè)項，構(gòu)造出反映線運動動態(tài)程度的特征量為：

最后，綜合陀螺儀和加速度計的信息，構(gòu)成既反映線運動又反映角運動動態(tài)程度的特征量為：

其中，c是角運動特征影響因子；影響因子c用于調(diào)節(jié)陀螺和加速度計的數(shù)據(jù)量級到等同的程度，使用序列方差的比值作為c的值；

綜上所述，特征量和Δ²都用于檢測載體運動的動態(tài)程度，通過設置特征量和Δ²閾值來達到對目標運動特性檢測的目的；

步驟3-4：ZUPT卡爾曼濾波器設計；

濾波器的誤差狀態(tài)選擇為九維，具體如下：

x＝[(δpⁿ)^T，(δvⁿ)^T，(φⁿ)^T]^T (22)

其中，x是誤差狀態(tài)向量，δpⁿ是位置誤差，δvⁿ是速度誤差，φⁿ是姿態(tài)失準角，三者都是SINS系統(tǒng)誤差；

將式(22)改寫成矩陣的形式，有：

其中，fⁿ×是fⁿ的反對稱陣，是陀螺儀的零偏，是隨機誤差；

由于IMU安裝在足部，當佩戴者腳跟落地時，IMU的實際地速為0，構(gòu)造觀測方程為：

其中，H_v＝[0_3×3 I_3×3 0_3×3]是速度域觀測矩陣，υ_v是速度測量誤差，是慣性導航系統(tǒng)輸出的地速；

離散時間狀態(tài)空間方程由式(23)和式(24)離散化得到，表示為：

其中，x_k是k時刻的狀態(tài)量，x_k-1是k-1時刻的狀態(tài)量，z_k是k時刻的觀測量，H_k是k時刻的觀測矩陣，Φ_k，k-1≈I+F(t_k-1)·τ是狀態(tài)轉(zhuǎn)移矩陣，F(xiàn)(t_k-1)是t_k-1時刻的誤差非線性函數(shù)，w_k-1是離散化的系統(tǒng)驅(qū)動噪聲，υ_k是觀測噪聲，τ是狀態(tài)更新的時間間隔；

為了保持濾波過程中估計始終無偏，初始狀態(tài)為P₀＝Cov(x₀)；

由于狀態(tài)向量全部是誤差向量，所以狀態(tài)的初始值為0，即對于初始狀態(tài)估計誤差的均方根矩陣P₀，描述如下：

其中，是初始失準角向量誤差的方差，和初始對準有關(guān)系；是初始速度誤差向量的方差，在靜止的情況下速度誤差為零，所以是初始位置誤差向量的方差，給定

按照下列公式及步驟進行濾波過程：

濾波過程完成后，得到誤差狀態(tài)的最小方差估計，誤差狀態(tài)校正對應狀態(tài)變量的公式如下：

其中，是當前歷元SINS系統(tǒng)的經(jīng)過位置，是當前歷元SINS系統(tǒng)的速度，是當前歷元SINS系統(tǒng)輸出的姿態(tài)矩陣。