本發(fā)明屬于體域網(wǎng)領(lǐng)域，涉及一種基于自適應(yīng)擴(kuò)展卡爾曼濾波的人體姿態(tài)識(shí)別方法。

背景技術(shù)：

人體姿態(tài)識(shí)別可以捕捉到某一時(shí)刻人體的空間運(yùn)動(dòng)信息或計(jì)算出面部、四肢、軀干的細(xì)微變形，在運(yùn)動(dòng)醫(yī)學(xué)、醫(yī)療康復(fù)、安全監(jiān)控、身份識(shí)別、人機(jī)交互、虛擬現(xiàn)實(shí)、機(jī)器人設(shè)計(jì)等領(lǐng)域具有廣泛應(yīng)用前景。姿態(tài)識(shí)別的常用方法有光學(xué)式、電磁式、聲學(xué)式和微機(jī)電式等幾種。光學(xué)姿態(tài)識(shí)別是計(jì)算機(jī)視覺的研究內(nèi)容之一，基本思想是跟蹤和監(jiān)視目標(biāo)的特定光點(diǎn)，然后在視頻圖像序列中將人體的運(yùn)動(dòng)部位抽取出來。電磁姿態(tài)識(shí)別方法利用電磁感應(yīng)原理，電磁發(fā)射源在空間產(chǎn)生規(guī)律變化的電磁場，安裝在人體上的接收傳感器能夠收到磁場信號，當(dāng)人體運(yùn)動(dòng)時(shí)，磁場信號發(fā)生變化，以此來進(jìn)行姿態(tài)識(shí)別。聲學(xué)姿態(tài)識(shí)別主要依靠檢測超聲波在移動(dòng)設(shè)備之間的反射時(shí)間來實(shí)現(xiàn)，這種方法要求至少有三個(gè)已知位置的固定參考設(shè)備。以上幾種識(shí)別方法，對工作環(huán)境要求較為苛刻，設(shè)計(jì)成本較高，為提高識(shí)別精度帶來很大限制。

隨著半導(dǎo)體技術(shù)的發(fā)展，微機(jī)電系統(tǒng)在姿態(tài)識(shí)別領(lǐng)域中得到越來越廣泛的應(yīng)用。微機(jī)電系統(tǒng)是指集微型機(jī)構(gòu)、微型傳感器、微型執(zhí)行器、信號處理和控制電路、接口、通信、電源等于一體的微型器件或系統(tǒng)，典型器件是陀螺儀、加速度計(jì)和電子羅盤，主要依靠慣性測量單元進(jìn)行姿態(tài)識(shí)別。采用微機(jī)電系統(tǒng)進(jìn)行姿態(tài)識(shí)別的主要難點(diǎn)在于需通過算法設(shè)計(jì)，克服傳感器輸出的累積誤差，以達(dá)到較高的識(shí)別精度。

1)人體姿態(tài)識(shí)別節(jié)點(diǎn)

本發(fā)明采用的人體姿態(tài)識(shí)別節(jié)點(diǎn)(以下簡稱為節(jié)點(diǎn))由傳感器模塊、無線通信模塊、處理器模塊和電源模塊四個(gè)部分組成。傳感器模塊集成了陀螺儀，型號為MPU3050；集成了加速度計(jì)和電子羅盤，型號為LSM303DLH。這3種傳感器分別負(fù)責(zé)采集角速度、加速度和磁場強(qiáng)度，并將這些傳感器數(shù)據(jù)從物理量轉(zhuǎn)換為電信號。無線通信模塊采用nRF24L01無線收發(fā)器，負(fù)責(zé)在2.4GHz～2.5GHz頻段內(nèi)與基站之間傳輸信息。處理器模塊采用STM32F103嵌入式微處理器，負(fù)責(zé)控制傳感器信號采集和無線收發(fā)功能。電源采用額定電壓為3.7V的鋰離子電池，負(fù)責(zé)為節(jié)點(diǎn)提供所需能量，節(jié)點(diǎn)提供鋰離子電池接口。

節(jié)點(diǎn)工作時(shí)，將信號通過無線方式傳輸至基站，基站由處理器模塊、無線通信模塊和串行接口模塊組成，負(fù)責(zé)接收節(jié)點(diǎn)發(fā)送的數(shù)據(jù)，并將接收到的數(shù)據(jù)通過串行接口傳輸至上位機(jī)。

2)四元數(shù)

四元數(shù)是一種超復(fù)數(shù)，由一個(gè)標(biāo)量和一個(gè)矢量組成，定義如式(1)所示：

其中a、b、c、d、n₀為標(biāo)量實(shí)數(shù)，i、j、k為三個(gè)虛部單位，為矢量。四元數(shù)也可表示為向量形式，即Q＝(a b c d)^T。如果Q滿足a²+b²+c²+d²＝1，則將Q稱作標(biāo)準(zhǔn)化四元數(shù)。

兩個(gè)四元數(shù)可以進(jìn)行乘法運(yùn)算。設(shè)Q₁＝a+bi+cj+dk，Q₂＝e+fi+gj+hk，則按照復(fù)數(shù)運(yùn)算法則計(jì)算，有i·i＝-1,i·j＝k,j·i＝-k......，因此有：

上述乘法也可表示為矩陣形式，即：

標(biāo)準(zhǔn)化四元數(shù)可用于描述向量的旋轉(zhuǎn)，設(shè)為某向量，Q為標(biāo)準(zhǔn)化四元數(shù)，定義如式(1)，Q^*為Q的共軛，定義如式(4)所示：

則可用Q^*和Q將向量旋轉(zhuǎn)至向量若為主動(dòng)旋轉(zhuǎn)，即向量逆時(shí)針圍繞旋轉(zhuǎn)軸旋轉(zhuǎn)，則該旋轉(zhuǎn)可通過式(5)來描述：

式(5)也可通過矩陣相乘來表示，如式(6)所示

其中矩陣C稱為方向余弦矩陣，是由兩組不同的標(biāo)準(zhǔn)正交基的基底向量之間的方向余弦所形成的矩陣，可用于向量在不同坐標(biāo)系間的坐標(biāo)轉(zhuǎn)換。矩陣C定義如下：

若為被動(dòng)旋轉(zhuǎn)，即對向量所在坐標(biāo)系本身進(jìn)行的逆時(shí)針旋轉(zhuǎn)，則可用式(8)來描述：

標(biāo)準(zhǔn)化四元數(shù)可用于描述剛體轉(zhuǎn)動(dòng)的角度，此時(shí)標(biāo)準(zhǔn)化四元數(shù)即為反應(yīng)剛體轉(zhuǎn)動(dòng)方式的姿態(tài)四元數(shù)。式(9)為姿態(tài)四元數(shù)對時(shí)間的導(dǎo)數(shù)與轉(zhuǎn)動(dòng)角速度之間的關(guān)系，即四元數(shù)運(yùn)動(dòng)學(xué)方程：

其中，為四元數(shù)對時(shí)間的導(dǎo)數(shù)，p為載體沿空間三維坐標(biāo)軸轉(zhuǎn)動(dòng)角速度構(gòu)造的四元數(shù)，其實(shí)部為零，即：

p＝0+ω_xi+ω_yj+ω_zk (10)

為求解包含姿態(tài)信息的四元數(shù)，需對進(jìn)行積分，然后將其化為姿態(tài)四元數(shù)，這樣便可得到描述從一種姿態(tài)轉(zhuǎn)動(dòng)至另一種姿態(tài)的等價(jià)姿態(tài)四元數(shù)。

設(shè)x_k＝a_k+b_ki+c_kj+d_kk為姿態(tài)四元數(shù)，其中k為非負(fù)整數(shù)，表示狀態(tài)序號。x_k用于描述系統(tǒng)在k時(shí)刻的位置狀態(tài)，初始狀態(tài)為x₀，其值設(shè)為1。p為代表系統(tǒng)當(dāng)前轉(zhuǎn)動(dòng)角速度的四元數(shù)，復(fù)數(shù)形式定義如式(10)所示。假設(shè)從k-1時(shí)刻到k時(shí)刻經(jīng)歷的時(shí)間為Δt，期間剛體做勻速轉(zhuǎn)動(dòng)，那么根據(jù)式(9)所示四元數(shù)運(yùn)動(dòng)學(xué)方程，將其離散化，可以列出式(11)：

其中為姿態(tài)四元數(shù)對時(shí)間的導(dǎo)數(shù)，定義如式(9)所示，即：

將(12)代入(11)得：

x_k＝x_k-1+0.5·x_k-1·p·Δt (13)

式(13)即為狀態(tài)向量的遞推公式，如果已知x₀和p，則系統(tǒng)全部姿態(tài)四元數(shù)可求。求得系統(tǒng)姿態(tài)四元數(shù)之后，利用四元數(shù)轉(zhuǎn)歐拉角的公式(14)，則可求得剛體在三維坐標(biāo)系中與三個(gè)坐標(biāo)軸的夾角，即得到剛體的當(dāng)前位置。

其中，α_k是k時(shí)刻剛體與x軸夾角，β_k是k時(shí)刻剛體與y軸夾角，γ_k是k時(shí)刻剛體與z軸夾角。

3)卡爾曼濾波

卡爾曼濾波是高效率的自回歸濾波器，能夠估算大范圍數(shù)據(jù)處理中的可變因素，可用于估算動(dòng)態(tài)系統(tǒng)的狀態(tài)。對于多維線性系統(tǒng)，式(15)為系統(tǒng)的狀態(tài)方程，式(16)為輸出方程：

x_k+1＝Ax_k+Bu_k+w_k (15)

z_k＝Hx_k+v_k (16)

其中A、B、H為系數(shù)矩陣，A稱作狀態(tài)轉(zhuǎn)移矩陣，H稱作觀測矩陣，x_k為系統(tǒng)當(dāng)前的狀態(tài)，x_k+1為系統(tǒng)下一時(shí)刻的狀態(tài)，u_k為系統(tǒng)當(dāng)前的輸入，z_k為系統(tǒng)當(dāng)前的輸出，w_k為當(dāng)前的過程噪聲，v_k為當(dāng)前的測量噪聲。式中的下標(biāo)k表示當(dāng)前時(shí)刻，k+1表示下一時(shí)刻。

假設(shè)過程噪聲w_k的平均值為零，測量噪聲v_k的平均值也為零，并且w_k和v_k相互獨(dú)立。過程噪聲的協(xié)方差S和測量噪聲的協(xié)方差R可以分別描述為式(17)和式(18)：

定義系統(tǒng)當(dāng)前時(shí)刻的最優(yōu)狀態(tài)估計(jì)值為狀態(tài)期望值為則系統(tǒng)先驗(yàn)誤差e'_k可表示如式(19)所示：

系統(tǒng)后驗(yàn)誤差e_k可表示如式(20)所示：

先驗(yàn)誤差的協(xié)方差如式(21)所示，即為誤差協(xié)方差的期望值。

后驗(yàn)誤差的協(xié)方差如式(22)所示：

根據(jù)卡爾曼濾波的基本原理，可由當(dāng)前狀態(tài)的最優(yōu)估計(jì)值推測下一時(shí)刻狀態(tài)的期望值，然后將系統(tǒng)測量誤差映射到狀態(tài)域，對狀態(tài)的期望值進(jìn)行修正，從而得到下一時(shí)刻的最優(yōu)估計(jì)值。按照如上方式系統(tǒng)不斷迭代，從而得到每一個(gè)狀態(tài)的最優(yōu)估計(jì)值。針對線性系統(tǒng)(15)和(16)的卡爾曼濾波方法由下面5個(gè)經(jīng)典公式表示：

P_k'＝AP_k-1A^T+S (24)

K_k＝P_k'H^T(HP_k'H^T+R)^-1 (25)

P_k＝P_k'-K_kHP_k' (27)

式(23)表示當(dāng)前時(shí)刻的狀態(tài)期望值可由前一時(shí)刻狀態(tài)及當(dāng)前輸入求得；式(24)表明了當(dāng)前時(shí)刻先驗(yàn)誤差的協(xié)方差的期望值可由前一時(shí)刻的后驗(yàn)誤差協(xié)方差求得；式(25)表明了卡爾曼增益K_k的求解方法；求得卡爾曼增益之后，可通過式(26)，由當(dāng)前時(shí)刻狀態(tài)期望值和當(dāng)前時(shí)刻觀測值計(jì)算得到當(dāng)前時(shí)刻狀態(tài)最優(yōu)估計(jì)值；為了使迭代能夠持續(xù)不斷進(jìn)行，還需要通過式(27)，利用當(dāng)前時(shí)刻先驗(yàn)誤差協(xié)方差計(jì)算求得后驗(yàn)誤差協(xié)方差，將其代入式(24)，即可求得下一時(shí)刻的先驗(yàn)誤差協(xié)方差，從而可以開始下一輪迭代。

卡爾曼濾波的5個(gè)經(jīng)典公式中，式(23)和(24)可稱為時(shí)間迭代公式，用于預(yù)測狀態(tài)值及誤差協(xié)方差；式(25)～(27)為測量迭代公式，通過觀測值修正期望值，從而獲取經(jīng)過改進(jìn)的期望值，即最優(yōu)估計(jì)值。式(26)說明了狀態(tài)期望值與輸出觀測值在最優(yōu)狀態(tài)估計(jì)值構(gòu)成中的比重，卡爾曼增益越大，輸出觀測值的比重越大，卡爾曼增益越小，最優(yōu)狀態(tài)估計(jì)值則越接近期望值。

自20世紀(jì)60年代以來，很多物理過程開始采用非線性模型進(jìn)行描述，卡爾曼濾波適用于處理線性系統(tǒng)在高斯噪聲下的狀態(tài)估計(jì)問題，對于非線性系統(tǒng)則不再適用。但是，如將非線性系統(tǒng)在其最優(yōu)估計(jì)值處進(jìn)行泰勒級數(shù)展開，只取其一階線性部分作為整體的近似表示，從而可將非線性系統(tǒng)轉(zhuǎn)化為近似線性系統(tǒng)。用于處理非線性系統(tǒng)的卡爾曼濾波方法稱作擴(kuò)展卡爾曼濾波，可用于處理非線性問題，得到系統(tǒng)的次優(yōu)狀態(tài)值。

對于某非線性系統(tǒng)，其狀態(tài)方程如(28)所示，輸出方程如(29)所示：

x_k+1＝f(x_k,u_k+1)+w_k (28)

z_k＝h(x_k)+v_k (29)

其中f(·)是一個(gè)非線性函數(shù)，可通過當(dāng)前時(shí)刻狀態(tài)值和下一時(shí)刻控制量計(jì)算下一時(shí)刻狀態(tài)值；h(·)也是一個(gè)非線性函數(shù)，通過當(dāng)前狀態(tài)計(jì)算系統(tǒng)輸出值。x_k為系統(tǒng)當(dāng)前狀態(tài)，x_k+1為系統(tǒng)下一時(shí)刻狀態(tài)，u_k為系統(tǒng)當(dāng)前輸入，z_k為系統(tǒng)當(dāng)前輸出，w_k為當(dāng)前過程噪聲，v_k為當(dāng)前測量噪聲。式中的下標(biāo)k表示當(dāng)前時(shí)刻，k+1表示下一時(shí)刻。過程噪聲協(xié)方差矩陣S、測量噪聲的協(xié)方差R、先驗(yàn)誤差e'_k、后驗(yàn)誤差e_k、先驗(yàn)誤差協(xié)方差P_k'、后驗(yàn)誤差協(xié)方差P_k的定義分別如式(17)-(22)所示。將f(·)和h(·)分別進(jìn)行泰勒級數(shù)展開，并只保留一階分量，即求f(·)和h(·)的雅克比矩陣，得到擴(kuò)展卡爾曼濾波的5個(gè)公式表示如下：

P_k'＝ΦP_k-1Φ^T+S (31)

K_k＝P_k'N^T(NP_k'N^T+R)^-1 (32)

P_k＝P_k'-K_kNP_k' (34)

其中，Φ是f(·)的雅克比矩陣，N是h(·)的雅克比矩陣。

在卡爾曼濾波過程中，一方面利用測量值不斷修正期望值，另一方面如果同時(shí)也對系統(tǒng)模型參數(shù)或噪聲統(tǒng)計(jì)參數(shù)進(jìn)行調(diào)整，則稱為自適應(yīng)卡爾曼濾波。例如，如果過程噪聲協(xié)方差矩陣或測量噪聲協(xié)方差矩陣完全未知或不能確切得知，則可在濾波過程中周期性修改過程噪聲協(xié)方差矩陣和測量噪聲協(xié)方差矩陣，進(jìn)而通過這兩個(gè)矩陣影響卡爾曼增益的大小，從而動(dòng)態(tài)改變?yōu)V波效果。

本發(fā)明基于四元數(shù)運(yùn)動(dòng)學(xué)方程建立系統(tǒng)模型，采用自適應(yīng)擴(kuò)展卡爾曼濾波方法進(jìn)行姿態(tài)識(shí)別。利用節(jié)點(diǎn)的陀螺儀實(shí)時(shí)檢測載體姿態(tài)，計(jì)算代表載體轉(zhuǎn)動(dòng)角度的姿態(tài)四元數(shù)，然后結(jié)合加速度計(jì)和電子羅盤輸出值進(jìn)行自適應(yīng)擴(kuò)展卡爾曼濾波處理，校準(zhǔn)該姿態(tài)四元數(shù)，最終得到比較準(zhǔn)確的人體姿態(tài)識(shí)別信息。

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

本發(fā)明要解決的技術(shù)問題是基于自適應(yīng)擴(kuò)展卡爾曼濾波，提出一種人體姿態(tài)識(shí)別方法。

本發(fā)明的技術(shù)方案：

一種基于自適應(yīng)擴(kuò)展卡爾曼濾波的人體姿態(tài)識(shí)別方法，步驟如下：

(1)模型設(shè)計(jì)

模型設(shè)計(jì)主要解決人體姿態(tài)識(shí)別的模型推導(dǎo)問題。人體運(yùn)動(dòng)時(shí)肢體的角度由姿態(tài)四元數(shù)來表示，采用擴(kuò)展卡爾曼濾波方法，通過四元數(shù)運(yùn)動(dòng)學(xué)方程，根據(jù)傳感器數(shù)據(jù)和當(dāng)前的姿態(tài)四元數(shù)，計(jì)算下一時(shí)刻姿態(tài)四元數(shù)的期望值；對于姿態(tài)四元數(shù)x_k＝(a_k b_k c_k d_k)^T，根據(jù)式(3)所示四元數(shù)乘法的性質(zhì)，可將式(13)所示四元數(shù)運(yùn)動(dòng)學(xué)方程的狀態(tài)向量遞推公式表示為式(35)，即姿態(tài)節(jié)點(diǎn)轉(zhuǎn)動(dòng)的狀態(tài)方程：

其中，Δt為從k-1時(shí)刻到k時(shí)刻經(jīng)歷的時(shí)間，ω_x、ω_y、ω_z為空間三個(gè)軸向的轉(zhuǎn)動(dòng)角速度，陀螺儀采集獲得；由于狀態(tài)方程本身是一個(gè)線性方程，狀態(tài)轉(zhuǎn)移矩陣為Φ，如式(36)所示，因此進(jìn)行擴(kuò)展卡爾曼濾波時(shí)不需對狀態(tài)轉(zhuǎn)移矩陣進(jìn)行泰勒級數(shù)展開。

假定已知k-1時(shí)刻姿態(tài)四元數(shù)的后驗(yàn)誤差協(xié)方差矩陣P_k-1，k時(shí)刻姿態(tài)四元數(shù)的先驗(yàn)誤差協(xié)方差矩陣P_k'由式(31)求得，如式(37)所示：

采用節(jié)點(diǎn)加速度和磁場強(qiáng)度作為觀測值，分別對狀態(tài)期望值進(jìn)行校正；由于狀態(tài)域和觀測域的數(shù)據(jù)類型不同，需要通過觀測函數(shù)h(·)將當(dāng)前姿態(tài)四元數(shù)轉(zhuǎn)換為載體坐標(biāo)系加速度；在初始時(shí)刻測得節(jié)點(diǎn)的加速度值將其代入式(8)，用當(dāng)前姿態(tài)四元數(shù)變換得到載體坐標(biāo)系的加速度過程如式(38)所示：

通過矩陣乘法可得觀測函數(shù)h(·)中各分量的表達(dá)式，如(39)所示：

按照式(40)求函數(shù)h(·)的雅克比矩陣，即令h(·)的各分量對四元數(shù)(a,b,c,d)的各項(xiàng)分別求偏導(dǎo)，可得式(41)。

式(41)中的矩陣J即為擴(kuò)展卡爾曼濾波的式(32)所示的矩陣N，將其帶入式(32)，即可求得卡爾曼增益K_k。

假定節(jié)點(diǎn)在當(dāng)前時(shí)刻只有重力加速度，在沒有任何誤差的理想情況下，經(jīng)過轉(zhuǎn)換后各個(gè)方向的加速度值等于當(dāng)前傳感器讀出的加速度值；采集姿態(tài)節(jié)點(diǎn)加速度計(jì)的輸出值作為系統(tǒng)輸出的觀測值z_k，將其與前述求得的卡爾曼增益K_k一同代入公式(33)，對狀態(tài)期望值進(jìn)行修正，得到k時(shí)刻狀態(tài)的次優(yōu)估計(jì)值

為了使迭代能夠持續(xù)進(jìn)行，需要計(jì)算下一時(shí)刻的協(xié)方差矩陣P_k，將式(41)求得的N、式(31)求得的P_k'，以及卡爾曼增益K_k代入式(34)，即可求得P_k。至此實(shí)現(xiàn)了一次完整迭代，完成了基于擴(kuò)展卡爾曼濾波算法的模型設(shè)計(jì)過程，通過該模型可得到人體運(yùn)動(dòng)的姿態(tài)四元數(shù)，從而完成人體姿態(tài)識(shí)別。

(2)參數(shù)設(shè)計(jì)

參數(shù)設(shè)計(jì)主要解決人體姿態(tài)識(shí)別系統(tǒng)的參數(shù)初始值和參數(shù)迭代計(jì)算問題，通過時(shí)變參數(shù)設(shè)計(jì)，將擴(kuò)展卡爾曼濾波方法演變?yōu)樽赃m應(yīng)擴(kuò)展卡爾曼濾波方法。參數(shù)設(shè)計(jì)部分要確定的參數(shù)分別為過程噪聲協(xié)方差矩陣S、測量噪聲協(xié)方差矩陣R、狀態(tài)初始值x₀，以及誤差協(xié)方差矩陣初始值P₀。

①過程噪聲協(xié)方差矩陣

當(dāng)前過程噪聲w_k由陀螺儀的非隨機(jī)噪聲和隨機(jī)噪聲兩部分構(gòu)成。其中，非隨機(jī)噪聲主要由傳感器品質(zhì)、安裝誤差和刻度系數(shù)誤差引起；隨機(jī)噪聲主要由逐次啟動(dòng)漂移、慢變漂移和快變漂移產(chǎn)生。在實(shí)際工作過程中，通過靜態(tài)標(biāo)定可以減弱非隨機(jī)噪聲和逐次啟動(dòng)漂移的影響，因此可近似認(rèn)為w_k僅包含慢變漂移和快變漂移產(chǎn)生的噪聲，并將其近似認(rèn)為是高斯白噪聲。

建立過程噪聲協(xié)方差矩陣的數(shù)學(xué)模型，將系統(tǒng)狀態(tài)方程表示為式(42)：

其中，誤差主要由等號右面第二項(xiàng)決定，即如式(43)所示：

其中，代表x軸角速度的誤差，代表y軸角速度的誤差，代表z軸角速度的誤差，Ψ_k-1為前一時(shí)刻狀態(tài)分量組成的4×4方陣；所以，將過程噪聲的協(xié)方差矩陣S表示如式(44)所示：

其中，ω_S為式中的協(xié)方差因子，如式(45)所示：

其中，為(45)中等式最右邊方陣中右下角的3×3方陣。由于Ψ_k-1由前一時(shí)刻狀態(tài)的各分量組成，因此在擴(kuò)展卡爾曼公式遞推過程中為已知，所以過程噪聲協(xié)方差矩陣最終歸結(jié)為計(jì)算即三軸角速度誤差的協(xié)方差矩陣。同時(shí)，也由于Ψ_k-1由前一時(shí)刻狀態(tài)的各分量組成，因此Ψ_k-1的值時(shí)時(shí)變化，因此本系統(tǒng)的處理方法實(shí)質(zhì)為自適應(yīng)擴(kuò)展卡爾曼濾波。

可按照下述方法估計(jì)：將節(jié)點(diǎn)置于水平桌面上保持靜止不動(dòng)，采集1000次陀螺儀的三軸角速度值，得到1000個(gè)三維向量，計(jì)算協(xié)方差矩陣，即可得到如式(46)所示：

將式(46)代入式(45)計(jì)算得到ω_S，如式(47)所示。

將式(47)代入式(44)，結(jié)合前一時(shí)刻狀態(tài)值，即可求出過程噪聲協(xié)方差矩陣S。

②測量噪聲協(xié)方差矩陣

v_k為當(dāng)前測量噪聲。在慣性導(dǎo)航系統(tǒng)中，測量噪聲可近似認(rèn)為是由一階馬爾科夫過程產(chǎn)生，近似等效為高斯白噪聲。

測量噪聲協(xié)方差矩陣按照如下方式來估計(jì)。首先按照六位置法對加速度計(jì)進(jìn)行標(biāo)定，然后將節(jié)點(diǎn)靜止放置于水平桌面，然后采集1000次加速度計(jì)的三軸輸出，得到1000個(gè)三維向量，計(jì)算協(xié)方差矩陣，得到式(48)：

從式(48)可知，非對角元值比對角元小1個(gè)數(shù)量級，考慮到測量精度及傳感器字長限制，可近似認(rèn)為加速度計(jì)三個(gè)軸向的測量噪聲相互獨(dú)立，因此可將所有非對角元取值為0，即得到加速度計(jì)的測量噪聲協(xié)方差矩陣如式(49)所示：

同理，將節(jié)點(diǎn)靜止放置于水平桌面，然后采集1000次電子羅盤的三軸輸出，得到1000個(gè)三維向量，計(jì)算協(xié)方差矩陣，得到式(50)：

基于同樣原因，可近似認(rèn)為電子羅盤三個(gè)軸向的測量噪聲相互獨(dú)立，因此可將所有非對角元取值為0，即得到電子羅盤的測量噪聲協(xié)方差矩陣如式(51)所示：

③濾波初始條件估算

假定系統(tǒng)一致完全隨機(jī)可控且一致完全隨機(jī)可觀，則卡爾曼濾波器具有一致漸近穩(wěn)定性，隨著濾波步數(shù)的增加，狀態(tài)初值x₀和誤差協(xié)方差矩陣初值P₀對狀態(tài)值x_k和P_k的影響將逐漸消失，使估計(jì)值趨向無偏，因此x₀和P₀的選擇范圍較為寬松。實(shí)際工作時(shí)，選擇初始狀態(tài)x₀為單位四元數(shù)(1,0,0,0)，選擇P₀初始值為單位陣。

本發(fā)明的有益效果在于，提出了一種新的人體姿態(tài)識(shí)別方法。該方法采集人體運(yùn)動(dòng)的角速度、加速度和人體周邊磁場強(qiáng)度數(shù)據(jù)，通過自適應(yīng)擴(kuò)展卡爾曼濾波進(jìn)行姿態(tài)解算，計(jì)算得到代表人體肢體運(yùn)動(dòng)角度的四元數(shù)，從而實(shí)現(xiàn)人體運(yùn)動(dòng)姿態(tài)識(shí)別。本方法可用于體育訓(xùn)練、醫(yī)療保健、游戲設(shè)計(jì)等領(lǐng)域。

附圖說明

圖1是本發(fā)明方法的流程圖。

圖2是本發(fā)明方法的x軸測量值補(bǔ)償前后誤差對比。

圖3是本發(fā)明方法的y軸測量值補(bǔ)償前后誤差對比。

圖4是本發(fā)明方法的z軸測量值補(bǔ)償前后誤差對比。

圖5是本發(fā)明方法的節(jié)點(diǎn)仿真模型隨動(dòng)驗(yàn)證結(jié)果，圖分別為節(jié)點(diǎn)轉(zhuǎn)動(dòng)到各個(gè)角度時(shí)，節(jié)點(diǎn)姿態(tài)與計(jì)算機(jī)中節(jié)點(diǎn)仿真模型姿態(tài)的對比圖。

圖6是本發(fā)明方法的人體仿真模型隨動(dòng)驗(yàn)證結(jié)果，圖①-⑨分別為佩戴節(jié)點(diǎn)后的各類型腿部姿態(tài)與計(jì)算機(jī)中人體仿真模型姿態(tài)的對比圖。

具體實(shí)施方式

以下結(jié)合發(fā)明內(nèi)容和說明書附圖詳細(xì)說明本發(fā)明的具體實(shí)施方式。

(1)方法概述

本發(fā)明采用的節(jié)點(diǎn)上包含陀螺儀、加速度計(jì)和電子羅盤等傳感器，分別用于測量節(jié)點(diǎn)轉(zhuǎn)動(dòng)的角速度、直線運(yùn)動(dòng)的加速度，以及節(jié)點(diǎn)位置的磁場強(qiáng)度。采用慣性傳感器設(shè)計(jì)的姿態(tài)識(shí)別節(jié)點(diǎn)是一個(gè)典型的非線性系統(tǒng)，離散狀態(tài)方程和輸出方程如式(28)和(29)所示。系統(tǒng)由于受到噪聲的干擾，無法精確得到狀態(tài)值，而只能在一定統(tǒng)計(jì)意義上對狀態(tài)值進(jìn)行估計(jì)。擴(kuò)展卡爾曼濾波適用于對非線性系統(tǒng)的隨機(jī)信號進(jìn)行處理，能夠從與被提取信號有關(guān)的測量值中估計(jì)出所需信號，測量值與被估計(jì)量之間的函數(shù)關(guān)系也已知，因此擴(kuò)展卡爾曼濾波適用于求解非線性隨機(jī)離散時(shí)間系統(tǒng)的狀態(tài)估計(jì)問題。同時(shí)，由于模型永遠(yuǎn)無法與真實(shí)系統(tǒng)完全等同，不可避免存在過程噪聲。對過程噪聲建模，將其分為恒定和時(shí)變兩個(gè)部分，其中時(shí)變部分在每個(gè)迭代周期均重新計(jì)算，因此采用的方式實(shí)際是一種自適應(yīng)擴(kuò)展卡爾曼濾波方法。

由于四元數(shù)可描述剛體運(yùn)動(dòng)規(guī)律，能夠?qū)⑷S空間的向量做任意旋轉(zhuǎn)。節(jié)點(diǎn)的加速度計(jì)用于采集載體坐標(biāo)系下三軸的加速度分量，可表示為三維向量形式。因此，如果在參考坐標(biāo)系下節(jié)點(diǎn)的加速度大小和方向不變，便可利用四元數(shù)建立起相鄰兩個(gè)時(shí)刻加速度向量之間的轉(zhuǎn)換關(guān)系。利用上一時(shí)刻陀螺儀輸出和狀態(tài)方程求得當(dāng)前時(shí)刻的狀態(tài)期望值，利用該期望值計(jì)算當(dāng)前時(shí)刻的加速度計(jì)輸出，然后采集當(dāng)前時(shí)刻的加速度計(jì)實(shí)際測量值，將二者進(jìn)行對比，利用其殘差和卡爾曼增益對當(dāng)前時(shí)刻狀態(tài)期望值進(jìn)行校正，得到狀態(tài)的估計(jì)值。節(jié)點(diǎn)的電子羅盤輸出同樣為三維向量形式，姿態(tài)校正過程與加速度計(jì)完全相同。系統(tǒng)經(jīng)過多次迭代，使姿態(tài)四元數(shù)的期望值不斷接近真實(shí)值，最終得到代表肢體運(yùn)動(dòng)角度的較為準(zhǔn)確的姿態(tài)四元數(shù)。

(2)模型設(shè)計(jì)

先不考慮人體姿態(tài)識(shí)別系統(tǒng)中參數(shù)的時(shí)變部分，在系統(tǒng)參數(shù)定常時(shí)，姿態(tài)識(shí)別問題可用擴(kuò)展卡爾曼濾波方法來處理。

擴(kuò)展卡爾曼濾波的第一個(gè)步驟是根據(jù)系統(tǒng)當(dāng)前狀態(tài)，通過函數(shù)f(·)計(jì)算下一狀態(tài)的預(yù)測值。在本問題中為通過四元數(shù)運(yùn)動(dòng)學(xué)方程建模，根據(jù)陀螺儀測得的角速度和當(dāng)前姿態(tài)四元數(shù)，計(jì)算下一時(shí)刻姿態(tài)四元數(shù)的期望值。

對于姿態(tài)四元數(shù)x_k＝(a_k b_k c_k d_k)^T，根據(jù)式(3)所示四元數(shù)乘法的性質(zhì)，可將遞推公式(13)表示為下式：

整理得：

這就是姿態(tài)節(jié)點(diǎn)轉(zhuǎn)動(dòng)的狀態(tài)方程，可知狀態(tài)方程本身即為線性方程，因此不需進(jìn)行泰勒級數(shù)展開。同時(shí)，由于姿態(tài)節(jié)點(diǎn)轉(zhuǎn)動(dòng)時(shí)沒有控制量，所以式(35)中不包括控制部分。由式(35)可知系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣Φ如式(36)所示：

式(36)中的轉(zhuǎn)動(dòng)角速度可用陀螺儀采集獲得，如果x_k-1為k-1時(shí)刻狀態(tài)的估計(jì)值則x_k為k時(shí)刻狀態(tài)的期望值可由式(30)可得到式(53)。

假定已知k-1時(shí)刻姿態(tài)四元數(shù)的后驗(yàn)誤差協(xié)方差矩陣P_k-1，k時(shí)刻姿態(tài)四元數(shù)的先驗(yàn)誤差協(xié)方差矩陣P_k'可由式(31)求得，如式(37)所示。

在經(jīng)過擴(kuò)展卡爾曼濾波時(shí)間迭代公式后，可根據(jù)陀螺儀信號得到當(dāng)前時(shí)刻姿態(tài)四元數(shù)的期望值。由于陀螺儀信號存在漂移，因而在積分過程中易產(chǎn)生累積誤差，需通過引入新的參考量減小累積誤差對姿態(tài)解算帶來的影響。

假設(shè)在采樣過程中，參考坐標(biāo)系的加速度、磁場強(qiáng)度不發(fā)生改變，在校正過程當(dāng)中，如果將當(dāng)前時(shí)刻姿態(tài)四元數(shù)的期望值作用于參考坐標(biāo)系下的加速度和磁場強(qiáng)度，使此二者進(jìn)行被動(dòng)旋轉(zhuǎn)，便可求得當(dāng)前時(shí)刻載體坐標(biāo)系加速度和磁場強(qiáng)度的期望值，進(jìn)而能夠與當(dāng)前時(shí)刻加速度和磁場強(qiáng)度的觀測值進(jìn)行比對求得殘差，以此殘差來校正姿態(tài)四元數(shù)。

為了借助觀測值對期望值進(jìn)行修正，首先應(yīng)求得卡爾曼增益K_k。采用節(jié)點(diǎn)加速度和磁場強(qiáng)度作為觀測值，分別對狀態(tài)期望值進(jìn)行校正，這兩種校正方式完全相同，下面以加速度為例說明。

由于狀態(tài)域和觀測域的數(shù)據(jù)類型不同，需要通過觀測函數(shù)h(·)將當(dāng)前姿態(tài)四元數(shù)轉(zhuǎn)換為載體坐標(biāo)系加速度。在初始時(shí)刻測得節(jié)點(diǎn)的加速度值將其帶入式(8)，用當(dāng)前姿態(tài)四元數(shù)變換得到載體坐標(biāo)系的加速度過程如式(38)所示：

通過矩陣乘法可得觀測函數(shù)h(·)中各分量的表達(dá)式，如(39)所示：

按照式(40)求函數(shù)h(·)的雅克比矩陣，即令h(·)的各分量對四元數(shù)(a,b,c,d)的各項(xiàng)分別求偏導(dǎo)，可得式(41)。

式(41)中的矩陣J即為擴(kuò)展卡爾曼濾波的式(32)所示的矩陣N，將其帶入式(32)，可求得卡爾曼增益K_k。

式(32)中，需要對矩陣NP_k'N^T+R求逆。矩陣可逆的充要條件是其行列式不為零，或矩陣滿秩，下面進(jìn)行分析。

在參數(shù)選擇時(shí)，選取測量噪聲協(xié)方差矩陣R為正定陣。P_k'是k時(shí)刻的先驗(yàn)誤差協(xié)方差矩陣，定義如(54)所示：

式中e'_k表示k時(shí)刻系統(tǒng)的期望值與真實(shí)值的差值，即先驗(yàn)誤差。根據(jù)式(55)所示協(xié)方差矩陣的性質(zhì)：

cov(Ux,Vx)＝Ucov(x,x)V^T (55)

可得式(56)：

NP_k'N^T＝Ncov(e'_k,e'_k)N^T＝cov(Ne'_k,Ne'_k) (56)

由該公式可知，NP_k'N^T是Ne'_k的協(xié)方差矩陣，因此NP_k'N^T至少為一個(gè)半正定陣，根據(jù)半正定陣的定義，可知存在任意一個(gè)非零向量W，使得式(57)成立：

WNP_k'N^TW^T≥0 (57)

又因?yàn)镽為正定矩陣，可知非零向量W使得式(58)成立：

WR W^T＞0 (58)

令R的維度為m，P_k'的維度為n，已知矩陣N是m×n矩陣，所以NP_k'N^T與R同為m階方陣。因此，將式(57)和式(58)相加，可得(59)：

因此，根據(jù)正定矩陣的定義可知，為NP_k'N^T+R正定矩陣，所以其必然可逆。

假定節(jié)點(diǎn)在當(dāng)前時(shí)刻只有重力加速度，在沒有任何誤差的理想情況下，經(jīng)過轉(zhuǎn)換后各個(gè)方向的加速度值應(yīng)該等于當(dāng)前傳感器讀出的加速度值。采集姿態(tài)節(jié)點(diǎn)加速度計(jì)的輸出值作為系統(tǒng)輸出的觀測值z_k，將其與前述求得的卡爾曼增益K_k一同代入公式(33)，對狀態(tài)期望值進(jìn)行修正，得到k時(shí)刻狀態(tài)的次優(yōu)估計(jì)值

為了使迭代能夠持續(xù)進(jìn)行，需要計(jì)算下一時(shí)刻的協(xié)方差矩陣P_k，將式(41)求得的N、式(37)求得的P_k'，以及卡爾曼增益K_k代入式(34)，即可求得P_k。至此，完成了基于擴(kuò)展卡爾曼濾波算法，采用加速度計(jì)觀測值對姿態(tài)四元數(shù)進(jìn)行修正的迭代過程。

(3)參數(shù)設(shè)計(jì)

在擴(kuò)展卡爾曼濾波的計(jì)算過程中，涉及到一系列參數(shù)初始化問題，分別為過程噪聲協(xié)方差矩陣S、測量噪聲協(xié)方差矩陣R、狀態(tài)初始值x₀，以及誤差協(xié)方差矩陣初始值P₀，下面分別進(jìn)行討論。

①過程噪聲協(xié)方差矩陣

在式(28)中，w_k為當(dāng)前過程噪聲，該噪聲主要由陀螺儀的非隨機(jī)噪聲和隨機(jī)噪聲兩部分構(gòu)成。其中，非隨機(jī)噪聲主要由傳感器品質(zhì)、安裝誤差和刻度系數(shù)誤差引起，是節(jié)點(diǎn)的固有特征，在一定時(shí)期內(nèi)基本保持不變，可采用靜態(tài)標(biāo)定的方法予以補(bǔ)償。陀螺儀的隨機(jī)噪聲主要由逐次啟動(dòng)漂移、慢變漂移和快變漂移產(chǎn)生。其中，逐次啟動(dòng)漂移體現(xiàn)為一個(gè)隨機(jī)常數(shù)，在陀螺儀工作過程中保持不變，因此可等效為常值偏差，也可通過標(biāo)定來消除。慢變漂移是由工作環(huán)境、電氣參數(shù)的隨機(jī)變化引起的，可近似用一階馬爾科夫過程描述，近似視為白噪聲。快變漂移表現(xiàn)為系統(tǒng)的雜亂無章的高頻跳變，也可抽象為白噪聲。

在系統(tǒng)實(shí)際工作過程中，通過靜態(tài)標(biāo)定可以減弱非隨機(jī)噪聲和逐次啟動(dòng)漂移的影響，因此可近似認(rèn)為w_k僅包含慢變漂移和快變漂移產(chǎn)生的噪聲，并將其近似認(rèn)為是高斯白噪聲。

基于以上分析，建立過程噪聲協(xié)方差矩陣的數(shù)學(xué)模型。由于本系統(tǒng)的狀態(tài)方程為線性方程，并且過程噪聲主要由陀螺儀引起，根據(jù)式(35)可知，ω_x、ω_y和ω_z是誤差的主要來源，因此可將式(35)轉(zhuǎn)換成式(42)：

式(42)中，等號右邊的各狀態(tài)分量已知，而且認(rèn)為是上一狀態(tài)的次優(yōu)估計(jì)值，則誤差部分主要由等號右面第二項(xiàng)決定，即誤差如式(43)所示：

其中，代表x軸角速度的誤差，代表y軸角速度的誤差，代表z軸角速度的誤差，Ψ_k-1為前一時(shí)刻狀態(tài)分量組成的4×4方陣。所以，將過程噪聲的協(xié)方差矩陣S表示如式(44)所示：

其中，ω_S為式中的協(xié)方差因子，如式(45)所示：

其中，為(45)中等式最右邊方陣中右下角的3×3方陣。由于Ψ_k-1由前一時(shí)刻狀態(tài)的各分量組成，因此在擴(kuò)展卡爾曼公式遞推過程中為已知，所以過程噪聲協(xié)方差矩陣最終歸結(jié)為計(jì)算即三軸角速度誤差的協(xié)方差矩陣。同時(shí)，也由于Ψ_k-1由前一時(shí)刻狀態(tài)的各分量組成，因此Ψ_k-1的值時(shí)時(shí)變化，因此本系統(tǒng)的處理方法實(shí)質(zhì)為自適應(yīng)擴(kuò)展卡爾曼濾波?？砂凑障率龇椒ü烙?jì)：將節(jié)點(diǎn)置于水平桌面上保持靜止不動(dòng)，采集1000次陀螺儀的三軸角速度值，得到1000個(gè)三維向量，計(jì)算協(xié)方差矩陣，即可得到如式(46)所示：

將式(46)代入式(45)計(jì)算得到ω_S，如式(47)所示。

將式(47)代入式(44)，結(jié)合前一時(shí)刻狀態(tài)值，即可求出過程噪聲協(xié)方差矩陣S。

②測量噪聲協(xié)方差矩陣

在式(29)中，v_k為當(dāng)前測量噪聲。在慣性導(dǎo)航系統(tǒng)中，測量噪聲可近似認(rèn)為是由一階馬爾科夫過程產(chǎn)生，近似等效為高斯白噪聲。

測量噪聲協(xié)方差矩陣按照如下方式來估計(jì)。首先按照六位置法對加速度計(jì)進(jìn)行標(biāo)定，然后將節(jié)點(diǎn)靜止放置于水平桌面，然后采集1000次加速度計(jì)的三軸輸出，得到1000個(gè)三維向量，計(jì)算協(xié)方差矩陣，得到式(48)：

從式(48)可知，非對角元值比對角元小1個(gè)數(shù)量級，考慮到測量精度及傳感器字長限制，可近似認(rèn)為加速度計(jì)三個(gè)軸向的測量噪聲相互獨(dú)立，因此可將所有非對角元取值為0，即得到加速度計(jì)的測量噪聲協(xié)方差矩陣如式(49)所示：

同理，將節(jié)點(diǎn)靜止放置于水平桌面，然后采集1000次電子羅盤的三軸輸出，得到1000個(gè)三維向量，計(jì)算協(xié)方差矩陣，得到式(50)：

基于同樣原因，可近似認(rèn)為電子羅盤三個(gè)軸向的測量噪聲相互獨(dú)立，因此可將所有非對角元取值為0，即得到電子羅盤的測量噪聲協(xié)方差矩陣如式(51)所示：

因此，本系統(tǒng)加速度計(jì)和電子羅盤的測量噪聲協(xié)方差矩陣分別采用(49)和(51)所示矩陣。

從式(49)和式(51)可知，加速度計(jì)和電子羅盤的測量噪聲協(xié)方差矩陣均為正定陣，因此前文中提及的NP_k'N^T+R可逆的前提條件存在。

③濾波初始條件估算

假定系統(tǒng)一致完全隨機(jī)可控且一致完全隨機(jī)可觀，則卡爾曼濾波器具有一致漸近穩(wěn)定性，隨著濾波步數(shù)的增加，狀態(tài)初值x₀和誤差協(xié)方差矩陣初值P₀對狀態(tài)值x_k和P_k的影響將逐漸消失，使估計(jì)值趨向無偏，因此x₀和P₀的選擇范圍較為寬松。實(shí)際工作時(shí)，只能對x₀和P₀初始值進(jìn)行估計(jì)選取，較為準(zhǔn)確的估計(jì)值能夠加快濾波收斂速度，反之則系統(tǒng)收斂速度變慢。本系統(tǒng)中，選擇初始狀態(tài)x₀為單位四元數(shù)(1,0,0,0)，選擇P₀初始值為單位陣，實(shí)驗(yàn)表明在該初始值設(shè)定下，系統(tǒng)狀態(tài)能夠較快收斂。

(4)實(shí)驗(yàn)及分析

采用自適應(yīng)擴(kuò)展卡爾曼濾波方法對姿態(tài)四元數(shù)角度進(jìn)行補(bǔ)償，之后與不進(jìn)行補(bǔ)償?shù)臄?shù)據(jù)進(jìn)行對比，了解補(bǔ)償效果。將姿態(tài)四元數(shù)傳輸至上位機(jī)，利用OpenGL設(shè)計(jì)節(jié)點(diǎn)仿真模型，實(shí)現(xiàn)對節(jié)點(diǎn)轉(zhuǎn)動(dòng)情況的復(fù)現(xiàn)，驗(yàn)證節(jié)點(diǎn)采集姿態(tài)信息的準(zhǔn)確性。利用OpenGL設(shè)計(jì)人體仿真模型，將節(jié)點(diǎn)固定在腿部，抬起腿部，觀察人體仿真模型的工作狀況，驗(yàn)證姿態(tài)識(shí)別準(zhǔn)確性。

①傳感器輸出補(bǔ)償

采用節(jié)點(diǎn)測量不同角度下讀數(shù)，判斷采用自適應(yīng)擴(kuò)展卡爾曼濾波方法進(jìn)行補(bǔ)償?shù)膶?shí)際效果。將節(jié)點(diǎn)固定在轉(zhuǎn)臺(tái)之上轉(zhuǎn)動(dòng)，近似認(rèn)為轉(zhuǎn)臺(tái)示數(shù)為真實(shí)值，傳感器讀數(shù)為測量值。從0度到1080度，每轉(zhuǎn)動(dòng)90度讀取兩次姿態(tài)四元數(shù)值，其中一次是未采用自適應(yīng)擴(kuò)展卡爾曼濾波方法求得的姿態(tài)四元數(shù)，另一次則經(jīng)過了補(bǔ)償，合計(jì)讀取26組姿態(tài)四元數(shù)。然后將二者分別轉(zhuǎn)換為x軸的角度。再將轉(zhuǎn)臺(tái)直立于水平面，按照上述步驟分別測量y軸和z軸的角度。合計(jì)讀取補(bǔ)償前后的姿態(tài)四元數(shù)各39個(gè)，按照四元數(shù)轉(zhuǎn)歐拉角的式(14)求得三軸對應(yīng)角度，并將每一個(gè)角度值與角度真實(shí)值相減求得誤差。補(bǔ)償前后的角度誤差如表1所示。

表1三軸輸出角度誤差補(bǔ)償前后對比

由表1可知，當(dāng)載體轉(zhuǎn)動(dòng)角度較小時(shí)，真實(shí)值和補(bǔ)償前后的測量值相差不大。隨著轉(zhuǎn)角不斷增加，未加補(bǔ)償?shù)臏y量值與真實(shí)值之間的誤差迅速增加，總體呈現(xiàn)正相關(guān)趨勢；而補(bǔ)償后的測量值始終較為接近真實(shí)值，在0～1080度測量區(qū)間內(nèi)，誤差始終在正負(fù)1度范圍內(nèi)。x軸、y軸和z軸在補(bǔ)償前后的誤差曲線分別如圖2、圖3和圖4所示，橫坐標(biāo)為轉(zhuǎn)動(dòng)角度，縱坐標(biāo)為誤差?？梢妼τ谌齻€(gè)坐標(biāo)軸來說，經(jīng)過補(bǔ)償后的曲線相較補(bǔ)償之前更接近真實(shí)值，大幅提高了系統(tǒng)測量準(zhǔn)確性。

②節(jié)點(diǎn)仿真模型隨動(dòng)驗(yàn)證

將節(jié)點(diǎn)計(jì)算得到的姿態(tài)四元數(shù)經(jīng)基站傳遞到上位機(jī)，采用OpenGL設(shè)計(jì)節(jié)點(diǎn)仿真模型來重現(xiàn)節(jié)點(diǎn)當(dāng)前轉(zhuǎn)動(dòng)角度，并利用四元數(shù)驅(qū)動(dòng)該節(jié)點(diǎn)仿真模型轉(zhuǎn)動(dòng)，節(jié)點(diǎn)仿真模型即圖5中顯示在計(jì)算機(jī)屏幕上的立方體。以不超過每秒90度的角速度隨機(jī)轉(zhuǎn)動(dòng)節(jié)點(diǎn)，觀察節(jié)點(diǎn)仿真模型隨動(dòng)轉(zhuǎn)動(dòng)情況，如圖5所示。

觀察實(shí)驗(yàn)現(xiàn)象，當(dāng)節(jié)點(diǎn)隨機(jī)轉(zhuǎn)動(dòng)時(shí)，顯示在計(jì)算機(jī)屏幕上的節(jié)點(diǎn)仿真模型能夠跟隨轉(zhuǎn)動(dòng)，說明節(jié)點(diǎn)能夠正確采集自身姿態(tài)，并將姿態(tài)信息傳遞到上位機(jī)，驅(qū)動(dòng)節(jié)點(diǎn)仿真模型同步轉(zhuǎn)動(dòng)。由于系統(tǒng)采樣及信息傳輸不可避免具有一定滯后，節(jié)點(diǎn)仿真模型的運(yùn)動(dòng)相比節(jié)點(diǎn)運(yùn)動(dòng)來說也有一定滯后，但是當(dāng)節(jié)點(diǎn)以不超過每秒90度左右的角速度轉(zhuǎn)動(dòng)時(shí)，肉眼基本觀察不到滯后。進(jìn)一步加快節(jié)點(diǎn)轉(zhuǎn)動(dòng)角速度時(shí)，可感覺到節(jié)點(diǎn)仿真模型運(yùn)動(dòng)滯后，但是一旦降低節(jié)點(diǎn)速度，節(jié)點(diǎn)仿真模型當(dāng)前轉(zhuǎn)動(dòng)角度會(huì)迅速向節(jié)點(diǎn)實(shí)際轉(zhuǎn)動(dòng)角度趨近，表明自適應(yīng)擴(kuò)展卡爾曼濾波算法的實(shí)際收斂速度很快。

③人體仿真模型隨動(dòng)驗(yàn)證

設(shè)計(jì)基于OpenGL的人體仿真模型來模擬測試者腿部轉(zhuǎn)動(dòng)。將姿態(tài)節(jié)點(diǎn)固定在測試者腿部，當(dāng)腿部轉(zhuǎn)動(dòng)時(shí)，姿態(tài)節(jié)點(diǎn)采集姿態(tài)四元數(shù)，然后通過wifi將四元數(shù)發(fā)送到基站，進(jìn)而傳輸?shù)竭\(yùn)行于上位機(jī)的人體仿真模型，驅(qū)動(dòng)人體仿真模型的腿部跟隨測試者腿部轉(zhuǎn)動(dòng)。實(shí)驗(yàn)情況如圖6所示。

當(dāng)腿部轉(zhuǎn)動(dòng)角速度低于每秒90度時(shí)，運(yùn)行于上位機(jī)的人體仿真模型腿部能夠同步轉(zhuǎn)動(dòng)，當(dāng)腿部轉(zhuǎn)動(dòng)角速度繼續(xù)增長時(shí)，人體仿真模型腿部轉(zhuǎn)動(dòng)開始出現(xiàn)滯后，但是一旦腿部運(yùn)動(dòng)角速度下降，人體仿真模型腿部的角度迅速趨近于測試者腿部的角度，說明自適應(yīng)擴(kuò)展卡爾曼濾波算法能夠快速收斂。