本發(fā)明涉及的是行人導(dǎo)航技術(shù)領(lǐng)域，特別涉及一種基于足底壓力檢測的零速修正方法。

背景技術(shù)：

在行人導(dǎo)航系統(tǒng)中，主要使用慣性測量方法測量行人的運(yùn)動軌跡。測量行人運(yùn)動軌跡的主要步驟包括：將陀螺儀和加速度計(jì)安裝在人體上，以獲取人體運(yùn)動時(shí)的運(yùn)動參數(shù)即角速度和加速度，從而根據(jù)求解速度、位置和姿態(tài)角的公式計(jì)算出行人的行走軌跡。陀螺儀、加速度計(jì)自身不可避免的誤差導(dǎo)致積分計(jì)算后的位置、速度等信息中包含了隨時(shí)間漂移的誤差，為了提高行人導(dǎo)航系統(tǒng)的定位精度，零速修正算法經(jīng)常被應(yīng)用于行人導(dǎo)航中。

零速修正算法本身具有局限性，主要包含靜止區(qū)間檢測不準(zhǔn)確和運(yùn)動區(qū)間的誤差累積。目前靜止區(qū)間檢測常用的方法是通過加速度計(jì)和陀螺儀的輸出數(shù)據(jù)來判斷行人運(yùn)動各時(shí)刻的加速度模值/角速度模值是否在閾值區(qū)間內(nèi)，但是，在高動態(tài)下利用加速度和角速度信息判斷靜止區(qū)間的方法容易出現(xiàn)誤判現(xiàn)象。近年來，足底動力學(xué)信息廣泛應(yīng)用于人體步態(tài)識別領(lǐng)域中，由足底動力學(xué)特點(diǎn)可知，足底所受壓力隨足部運(yùn)動狀態(tài)的變化而變化，在行走過程中足底所受的總壓力一般呈駝峰形，根據(jù)行走過程中足底所受壓力與足部狀態(tài)的映射關(guān)系，通過足底壓力值可以較準(zhǔn)確的檢測出靜止區(qū)間。

導(dǎo)航過程中，加速度計(jì)輸出值、陀螺儀輸出值和解算出的位置、速度、姿態(tài)角等信息通常任何時(shí)刻都帶有噪聲，影響整個導(dǎo)航系統(tǒng)的定位精度?？柭鼮V波技術(shù)利用動態(tài)量測信息去除噪聲的影響，估計(jì)出從某種統(tǒng)計(jì)意義上誤差最小的狀態(tài)估值。對動態(tài)行為的估計(jì)，它能實(shí)現(xiàn)實(shí)時(shí)運(yùn)行狀態(tài)的估計(jì)和預(yù)測。針對現(xiàn)有的零速修正方法存在的缺陷，本發(fā)明提供了一種應(yīng)用于行人導(dǎo)航中基于足底壓力檢測的零速修正方法，實(shí)現(xiàn)較復(fù)雜運(yùn)動狀態(tài)下靜態(tài)區(qū)間的檢測，并通過卡爾曼濾波實(shí)現(xiàn)行人導(dǎo)航系統(tǒng)的誤差修正以提高行人導(dǎo)航系統(tǒng)定位精度。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的是針對現(xiàn)有技術(shù)不足提供一種行人導(dǎo)航中基于足底壓力檢測的零速修正方法，其主要特征在于結(jié)合足底壓力值與慣性測量元件的輸出數(shù)據(jù)可以更加準(zhǔn)確的判定靜止區(qū)間，通過卡爾曼濾波實(shí)現(xiàn)對行人導(dǎo)航系統(tǒng)中的誤差修正，相對于現(xiàn)有的零速修正算法而言，結(jié)合足底壓力檢測的零速修正算法考慮到了人體行走的行為模式和足部運(yùn)動狀態(tài)。

本發(fā)明所述方法包括下列步驟：

1)在人體足底的前腳掌和后腳跟安裝壓力傳感器，實(shí)時(shí)采集運(yùn)動時(shí)前腳掌與后腳跟的壓力值，將微型慣性測量單元即MIMU固定在人體腳部的踝關(guān)節(jié)上方，采集運(yùn)動過程中的加速度和角速度信息，同時(shí)減少人體行走狀態(tài)對MIMU的影響；

2)根據(jù)人體步態(tài)相位的分析，結(jié)合每個離散時(shí)刻足底壓力值、加速度值、角速度值以及噪聲特性設(shè)定靜止區(qū)間的上、下閾值；

3)根據(jù)足底壓力變化與步態(tài)變化的內(nèi)在聯(lián)系，結(jié)合足底壓力傳感器、加速度計(jì)和陀螺儀分別設(shè)定判定腳掌全部落地即靜止區(qū)間的條件，并根據(jù)這些條件所進(jìn)行的與操作，最終判定出靜止區(qū)間；

4)零速修正在靜止區(qū)間內(nèi)觸發(fā)卡爾曼濾波器，在檢測出的靜止區(qū)間內(nèi)人體腳部的運(yùn)動速度視為零，以此時(shí)MIMU所輸出的速度值作為卡爾曼濾波器的量測量，利用卡爾曼濾波估計(jì)更多的誤差參數(shù)，從而修正基于MIMU的慣性行人導(dǎo)航系統(tǒng)中的速度誤差、位置誤差和姿態(tài)誤差。

所述步驟2)中的步態(tài)相位為行走時(shí)的支撐相位和擺動相位，由步態(tài)相位與足底受力情況內(nèi)在聯(lián)系的分析，可知行人處于靜止區(qū)間時(shí)其支撐腳所受的力為人體總質(zhì)量，根據(jù)人體體重和單位窗口內(nèi)的足底平均壓力值設(shè)定閾值F_V；根據(jù)理想情況下靜止區(qū)間內(nèi)腳部的合加速度大小為G即重力加速度，且方向垂直地面向下這一特點(diǎn)，設(shè)定加速度幅值的判斷閾值為[G_V1,G_V2]；根據(jù)單位窗口內(nèi)平均加速度幅值與噪聲特性設(shè)定加速度幅值標(biāo)準(zhǔn)差的判斷閾值為G_V3；根據(jù)單位窗口內(nèi)角速度幅值與噪聲特性設(shè)定角速度幅值的判斷閾值為ω_V；

所述步驟3)中，設(shè)定檢測靜止區(qū)間的4個判斷條件分別為C₁、C₂、C₃和C₄。當(dāng)行人處于靜止區(qū)間內(nèi)，腳掌完全貼地，前后掌均受到地面反作用力且支撐腳所受力為人體自重，因此，設(shè)定判斷條件C₁為

F₁＝f₁＞0&f₂＞0

F₂＝f₁+f₂

式中，f₁為前腳掌所受力，f₂為后腳跟所受力，F(xiàn)₁表示前腳掌與后腳跟均受力時(shí)為真，否則為假，F(xiàn)₂為前后掌總受力。判斷條件C₂為通過加速度模值進(jìn)行判定，如下

式中，a_kx、a_ky、a_kz分別為加速度沿三軸方向上的分量，|a_k|為加速度模值。判斷條件C₃根據(jù)加速度矢量的幅值方差進(jìn)行判定

是k時(shí)刻的平均加速度幅值，s為平滑窗口長度，a_j為加速度采樣點(diǎn)，則k時(shí)刻加速度的幅值方差為

式中，δ(a_k)為k時(shí)刻加速度的幅值標(biāo)準(zhǔn)差，G_V3為判定閾值。利用角速度幅值設(shè)定判斷條件C₄，角速度幅值表示為

ω_kx、ω_ky、ω_kz分別表示角速度在三軸方向上的分量，|ω_k|為角速度模值，則判斷條件C₄表達(dá)式為

對C₁、C₂、C₃和C₄條件進(jìn)行與操作，最終的靜止區(qū)間的判定條件為：C＝C₁&C₂&C₃&C₄，當(dāng)四個條件都滿足時(shí)，C記錄為1，表示檢測到靜止區(qū)間，否則記錄為0，表示腳部處于運(yùn)動狀態(tài)。

所述步驟4)中，通過利用步驟2)、3)所述的靜態(tài)區(qū)間檢測方法檢測出高動態(tài)下行人導(dǎo)航中的靜態(tài)區(qū)間，將此時(shí)刻的運(yùn)動速度視為零，MIMU在靜止區(qū)間輸出的速度值即為MIMU的誤差漂移量，以該時(shí)刻MIMU所測得的速度值作為量測量通過卡爾曼濾波對其他誤差參數(shù)進(jìn)行估計(jì)并修正。

卡爾曼濾波離散化后的狀態(tài)方程和量測方程分別為：

式中，X_k、X_k-1分別表示k時(shí)刻、k-1時(shí)刻的狀態(tài)估計(jì)；Z_k為離散化系統(tǒng)觀測矩陣；φ_k,k-1為離散化狀態(tài)轉(zhuǎn)移矩陣；H_k為離散化系統(tǒng)量測矩陣；W_k-1和V_k分別為離散化系統(tǒng)狀態(tài)噪聲向量和量測噪聲向量；

K時(shí)刻的狀態(tài)一步預(yù)測值X_k,k-1為：

X_k,k-1＝φ_k,k-1X_k-1

濾波增益K_k為：

k時(shí)刻狀態(tài)估計(jì)值X_k為：

X_k＝X_k,k-1+K_k(Z_k-H_kX_k,k-1)

一步預(yù)測均方誤差P_k,k-1為：

估計(jì)均方誤差協(xié)方差矩陣P_k,k為：

式中，P_k-1,k-1表示k-1時(shí)刻的均方誤差，Q_k-1表示系統(tǒng)噪聲協(xié)方差矩陣，R_k表示量測噪聲協(xié)方差矩陣，I表示單位矩陣；

通過以靜止區(qū)間的速度作為量測值，利用卡爾曼濾波估計(jì)出位置、速度和姿態(tài)的狀態(tài)誤差后，對位置、速度和姿態(tài)信息進(jìn)行補(bǔ)償?shù)玫礁鼮榫_的定位信息。

本發(fā)明的有益效果：通過分析足底壓力與步態(tài)相位內(nèi)在聯(lián)系，從加速度、陀螺儀和足底受力等方面入手，設(shè)定多個靜止區(qū)間閾值與判定條件，提高了檢測靜止區(qū)間的準(zhǔn)確性，有助于高動態(tài)下的靜止區(qū)間的檢測。利用靜止區(qū)間內(nèi)MIMU的速度值作為量測值并通過卡爾曼濾波對速度、位置和姿態(tài)誤差的修正，可以提高行人導(dǎo)航的定位精度。本發(fā)明方法將足底壓力傳感器應(yīng)用于零速修正中，有效的提高了零速區(qū)間檢測的精度，符合了人體運(yùn)動生物力學(xué)并提高了行人導(dǎo)航的定位精度。

附圖說明

圖1是本發(fā)明所述足底壓力傳感器安裝位置圖；

圖中：A為后腳跟足底壓力傳感器安裝位置；B為前腳掌足底村力傳感器安裝位置；

圖2是本發(fā)明所述MIMU安裝位置圖；

圖中：C為MIMU安裝位置

圖3是本發(fā)明所述步態(tài)相位圖；

圖中：D為支撐期E為擺動期；

圖4是本發(fā)明所述卡爾曼濾波誤差修正圖。

具體實(shí)施方式

為了實(shí)現(xiàn)高動態(tài)下的零速修正，本發(fā)明提供了一種基于足底壓力檢測零速修正方法。下面結(jié)合附圖對發(fā)明的技術(shù)方案進(jìn)行詳細(xì)的說明：

(1)在人體足底的前腳掌和后腳跟安裝壓力傳感器，實(shí)時(shí)采集運(yùn)動時(shí)前腳掌與后腳跟的壓力值，將MIMU固定在人體腳部的踝關(guān)節(jié)處，采集運(yùn)動過程中的加速度和角速度信息；

在導(dǎo)航開始之前，將足底壓力傳感器與MIMU安裝在準(zhǔn)確位置上，根據(jù)人體行走時(shí)腳掌所受壓力區(qū)間主要為前腳掌和后腳跟兩個部位的特點(diǎn)，本發(fā)明分別在人體足底前腳掌和后腳跟處各安裝一個足底壓力傳感器，安裝方法如圖1所示，其中B處為前腳掌安裝位置，A處為后腳跟安裝位置。圖2所示c處為本發(fā)明中MIMU的具體安裝位置，在人體腳踝上方安裝MIMU以此減小腳面運(yùn)動對MIMU的影響。

(2)根據(jù)人體步態(tài)相位的分析，結(jié)合每個離散時(shí)刻足底壓力值、加速度值、角速度值及噪聲特性設(shè)定判定為靜止區(qū)間的上、下閾值；

結(jié)合行走過程中的步態(tài)相位與足底受力分析設(shè)定足壓的判斷閾值，其中步態(tài)相位分為支撐相位和擺動相位(如圖3所示)，圖中D為支撐相位，E為擺動相位，支撐相位又分為支撐前期、支撐中期和支撐后期；由步態(tài)相位與足底受力情況內(nèi)在聯(lián)系，可知行人處于靜止區(qū)間時(shí)其支撐腳所受的力為人體總質(zhì)量，結(jié)合圖3以右腳為研究對象，當(dāng)處于靜止區(qū)間時(shí)，為單腳支撐期即支撐前期，則右腳承受人體總質(zhì)量。根據(jù)人體體重和單位窗口內(nèi)的足底平均壓力值設(shè)定閾值F_V；根據(jù)理想情況下靜止區(qū)間內(nèi)腳部的合加速度大小為G(重力加速度)且方向垂直地面向下這一特點(diǎn)，設(shè)定加速度幅值的判斷閾值為[G_V1,G_V2]；根據(jù)單位窗口內(nèi)平均加速度幅值與噪聲特性設(shè)定加速度幅值標(biāo)準(zhǔn)差的判斷閾值為G_V3；根據(jù)單位窗口內(nèi)角速度幅值與噪聲特性設(shè)定角速度幅值的判斷閾值為ω_V。

(3)根據(jù)足底壓力變化與步態(tài)變化的內(nèi)在聯(lián)系，結(jié)合足底壓力傳感器、加速度計(jì)和陀螺儀設(shè)定若干個判定腳掌全部落地即靜止區(qū)間的條件，并根據(jù)這些條件所進(jìn)行的與操作，最終判定出靜止區(qū)間。

首先，設(shè)定檢測靜止區(qū)間的4個判斷條件分別為C₁、C₂、C₃和C₄。當(dāng)行人處于靜止區(qū)間內(nèi)，其腳掌完全貼地，前后掌均受到地面反作用力且支撐腳所受力為人體自重，因此，設(shè)定判斷條件C₁為

F₁＝f₁＞0&f₂＞0

F₂＝f₁+f₂

式中，f₁為前腳掌所受力，f₂為后腳跟所受力，F(xiàn)₁表示前腳掌與后腳跟均受力時(shí)為真，否則為假，F(xiàn)₂為前后掌總受力。判斷條件C₂為通過加速度模值進(jìn)行判定，如下

式中，a_kx、a_ky、a_kz分別為加速度沿三軸方向上的分量，|a_k|為加速度模值。判斷條件C₃根據(jù)加速度矢量的幅值方差進(jìn)行判定

是k時(shí)刻的平均加速度幅值，s為平滑窗口長度，a_j為加速度采樣點(diǎn)，則k時(shí)刻加速度的幅值方差為

式中，δ(a_k)為k時(shí)刻加速度的幅值標(biāo)準(zhǔn)差，G_V3為判定閾值。利用角速度幅值設(shè)定判斷條件C₄，角速度幅值表示為

ω_kx、ω_ky、ω_kz分別表示角速度在三軸方向上的分量，|ω_k|為角速度模值，則判斷條件C₄表達(dá)式為

對C₁、C₂、C₃和C₄條件進(jìn)行與操作，最終的靜止區(qū)間的判定條件為：C＝C₁&C₂&C₃&C₄，當(dāng)四個條件都滿足時(shí)，C記錄為1，表示檢測到靜止區(qū)間，否則記錄為0，表示腳部處于運(yùn)動狀態(tài)。

(4)將導(dǎo)航實(shí)驗(yàn)過程中采集到的足壓數(shù)據(jù)、加速度數(shù)據(jù)以及角速度數(shù)據(jù)通過所述步驟(3)中提出的4個判斷條件C₁、C₂、C₃和C₄對靜止區(qū)間進(jìn)行檢測。

在檢測出的靜止區(qū)間內(nèi)觸發(fā)卡爾曼濾波器，將靜止區(qū)間的運(yùn)動速度視為零，則MIMU在靜止區(qū)間輸出的速度值即為MIMU的誤差漂移量，以該時(shí)刻MIMU所測得的速度值作為量測量通過卡爾曼濾波器對其他誤差參數(shù)進(jìn)行估計(jì)并修正，其系統(tǒng)框圖如圖4所示。

卡爾曼濾波離散化后的狀態(tài)方程和量測方程分別為：

式中，X_k、X_k-1分別表示k時(shí)刻、k-1時(shí)刻的狀態(tài)估計(jì)；Z_k為離散化系統(tǒng)觀測矩陣；φ_k,k-1為離散化狀態(tài)轉(zhuǎn)移矩陣；H_k為離散化系統(tǒng)量測矩陣；W_k-1和V_k分別為離散化系統(tǒng)狀態(tài)噪聲向量和量測噪聲向量；

K時(shí)刻的狀態(tài)一步預(yù)測值X_k,k-1為：

X_k,k-1＝φ_k,k-1X_k-1

濾波增益K_k為：

k時(shí)刻狀態(tài)估計(jì)值X_k為：

X_k＝X_k,k-1+K_k(Z_k-H_kX_k,k-1)

一步預(yù)測均方誤差P_k,k-1為：

估計(jì)均方誤差協(xié)方差矩陣P_k,k為：

式中，P_k-1,k-1表示k-1時(shí)刻的均方誤差，Q_k-1表示系統(tǒng)噪聲協(xié)方差矩陣，R_k表示量測噪聲協(xié)方差矩陣，I表示單位矩陣；

通過以靜止區(qū)間的速度作為量測值，利用卡爾曼濾波估計(jì)出位置、速度和姿態(tài)的狀態(tài)誤差后，對位置、速度和姿態(tài)信息進(jìn)行補(bǔ)償?shù)玫礁鼮榫_的定位信息。

上述給出了本發(fā)明的一個具體實(shí)施方案，本發(fā)明書中未作詳細(xì)描述的內(nèi)容屬于本領(lǐng)域?qū)I(yè)技術(shù)人員公知的現(xiàn)有技術(shù)。

上面結(jié)合附圖對本發(fā)明的實(shí)施方式作了說明，但本發(fā)明并不局限于上述實(shí)施方式，在本領(lǐng)域的技術(shù)人員所具備的知識范圍內(nèi)，還可以在不脫離本發(fā)明宗旨的前提下做出各種變化。