[發(fā)明專利]一種基于自回溯算法的室內(nèi)行人導(dǎo)航方法

有效

• 申請?zhí)枺?/td>: 201811596821.0
• 申請日: 2018-12-26
• 公開（公告）號: CN109855620B
• 公開（公告）日: 2020-10-09
• 發(fā)明（設(shè)計(jì)）人: 梁源;徐兵
• 申請（專利權(quán)）人: 北京壹?xì)淇萍加邢薰?/a>
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 北京國林貿(mào)知識產(chǎn)權(quán)代理有限公司 11001
• 代理人: 李桂玲;杜國慶

1.一種基于自回溯算法的室內(nèi)行人導(dǎo)航方法，被導(dǎo)航的行人佩戴有MEMS傳感器，其特征在于，所述導(dǎo)航方法的步驟包括：

步驟1：根據(jù)MEMS陀螺儀和MEMS加速度計(jì)進(jìn)行行人步態(tài)推算得到一段時長行人的行走信息，對該時間段內(nèi)的行走信息進(jìn)行直線檢測，

步驟2：判定行人在該時間段內(nèi)的行走路線是否為直線，如果確定行人行走處于直線狀態(tài)，則執(zhí)行步驟3，否則返回步驟1，所述行走信息包括行人位置、航向角和步長；

步驟3：根據(jù)航向角信息構(gòu)建偽觀測量，利用正反向自適應(yīng)卡爾曼濾波算法進(jìn)行導(dǎo)航誤差檢測與修正補(bǔ)償；

步驟4：重復(fù)步驟1至3，持續(xù)進(jìn)行導(dǎo)航誤差檢測與修正補(bǔ)償，同時輸出行人室內(nèi)導(dǎo)航定位結(jié)果；

所述構(gòu)建偽觀測量，利用正反向自適應(yīng)卡爾曼濾波算法進(jìn)行導(dǎo)航誤差檢測與修正補(bǔ)償?shù)牟襟E是：

第一步，利用PDR推算分別獲取時長內(nèi)每一步的航向角信息：

獲取時長內(nèi)每一步的步長信息：

第二步，構(gòu)建偽觀測序列：

第三步，建立與每一步的航向角信息、每一步的步長信息、偽觀測序列信息相關(guān)聯(lián)的表達(dá)式模型：

H＝[0 0 0 0 1]，

A(k)為k時刻的系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣，H為量測矩陣；

第四步，對表達(dá)式模型利用自適應(yīng)卡爾曼濾波算法進(jìn)行導(dǎo)航誤差檢測與修正補(bǔ)償，具體的算法是：

P_k,k-1＝A(k)P_k-1(A(k))^T+Q

P_k＝[I-K_kH]P_k,k-1

其中：

代表濾波結(jié)果，代表上一步濾波結(jié)果，Q代表模型噪聲對應(yīng)的協(xié)方差矩陣，代表濾波結(jié)果狀態(tài)上一步預(yù)測，P_k,k-1代表上一步預(yù)測濾波誤差方差，為主航向，K_k代表濾波增益，P_k代表當(dāng)前步濾波誤差方差，P_k-1代表k-1步的濾波誤差方差，I代表對應(yīng)維度的單位矩陣，代表自適應(yīng)量測噪聲方差矩陣：其中std(Z_pseduo)代表序列Z_pseduo的標(biāo)準(zhǔn)差；其中，會隨著行人行走模式、行走狀態(tài)的變化進(jìn)行自適應(yīng)在線調(diào)整；

第五步，在第四步的基礎(chǔ)上實(shí)現(xiàn)正反向自適應(yīng)卡爾曼濾波算法進(jìn)行導(dǎo)航誤差檢測與修正補(bǔ)償，具體的算法是：

首次正向自適應(yīng)濾波：

設(shè)定正向?yàn)V波的初始值為：

開始進(jìn)行正向自適應(yīng)濾波：

P_k＝[I-K_kH]P_k,k-1

其中，k由1遞增至10；得到正向?yàn)V波結(jié)果：與

進(jìn)行反向自適應(yīng)濾波：

將與作為濾波初始值，

P_k-1,k＝A(k-1)^-1P_k(A(k-1)^-1)^T+Q

P_k-1＝[I-K_kH]P_k-1,k

其中，A(k-1)^-1代表A(k-1)的逆矩陣，k在反向?yàn)V波過程中由10遞減至1；得到反向?yàn)V波結(jié)果與P₁²；

此時結(jié)束一輪正反向自適應(yīng)Kalman濾波，下一輪運(yùn)算開始時，將與P₁²提供給正向自適應(yīng)濾波算法繼續(xù)重復(fù)上述運(yùn)算過程，循環(huán)迭代兩輪后，最終得到正反向自適應(yīng)濾波算法的結(jié)果：與

2.根據(jù)權(quán)利要求1所述的導(dǎo)航方法，其特征在于，所述時長是行人行走10步的時間長度。

3.根據(jù)權(quán)利要求1所述的導(dǎo)航方法，其特征在于，所述直線檢測及判定行人在該時間段內(nèi)的行走路線是否為直線是：計(jì)算時長起始和終止之間航向角最大值與最小值的差值，如果差值的絕對值小于20*π/180弧度，則認(rèn)為行人在該時間段內(nèi)為直線行走，否則不為直線行走。