一種基于自適應(yīng)擴(kuò)展卡爾曼濾波的人體姿態(tài)識(shí)別方法技術(shù)

本發(fā)明專利技術(shù)公開了一種基于自適應(yīng)擴(kuò)展卡爾曼濾波的人體姿態(tài)識(shí)別方法，屬于體域網(wǎng)領(lǐng)域。該方法分為兩個(gè)部分，模型設(shè)計(jì)和參數(shù)設(shè)計(jì)。模型設(shè)計(jì)中，利用四元數(shù)可反映人體肢體運(yùn)動(dòng)角度的特點(diǎn)，通過(guò)慣性傳感器采集人體運(yùn)動(dòng)的角速度、加速度和周邊磁場(chǎng)強(qiáng)度，基于自適應(yīng)擴(kuò)展卡爾曼濾波方法進(jìn)行姿態(tài)解算，求得姿態(tài)四元數(shù)。參數(shù)設(shè)計(jì)中，通過(guò)理論分析和實(shí)驗(yàn)方法確定了過(guò)程噪聲協(xié)方差矩陣、測(cè)量噪聲協(xié)方差矩陣的取值，以及狀態(tài)初始值和狀態(tài)協(xié)方差矩陣初始值，使自適應(yīng)擴(kuò)展卡爾曼濾波方法可以持續(xù)迭代進(jìn)行，從而不斷實(shí)時(shí)識(shí)別人體運(yùn)動(dòng)姿態(tài)。本發(fā)明專利技術(shù)可作為體育訓(xùn)練、醫(yī)療保健、游戲設(shè)計(jì)等領(lǐng)域的人體姿態(tài)識(shí)別方法使用。

【技術(shù)實(shí)現(xiàn)步驟摘要】

本專利技術(shù)屬于體域網(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ī)視覺的研究?jī)?nèi)容之一，基本思想是跟蹤和監(jiān)視目標(biāo)的特定光點(diǎn)，然后在視頻圖像序列中將人體的運(yùn)動(dòng)部位抽取出來(lái)。電磁姿態(tài)識(shí)別方法利用電磁感應(yīng)原理，電磁發(fā)射源在空間產(chǎn)生規(guī)律變化的電磁場(chǎng)，安裝在人體上的接收傳感器能夠收到磁場(chǎng)信號(hào)，當(dāng)人體運(yùn)動(dòng)時(shí)，磁場(chǎng)信號(hào)發(fā)生變化，以此來(lái)進(jìn)行姿態(tài)識(shí)別。聲學(xué)姿態(tài)識(shí)別主要依靠檢測(cè)超聲波在移動(dòng)設(shè)備之間的反射時(shí)間來(lái)實(shí)現(xiàn)，這種方法要求至少有三個(gè)已知位置的固定參考設(shè)備。以上幾種識(shí)別方法，對(duì)工作環(huán)境要求較為苛刻，設(shè)計(jì)成本較高，為提高識(shí)別精度帶來(lái)很大限制。隨著半導(dǎo)體技術(shù)的發(fā)展，微機(jī)電系統(tǒng)在姿態(tài)識(shí)別領(lǐng)域中得到越來(lái)越廣泛的應(yīng)用。微機(jī)電系統(tǒng)是指集微型機(jī)構(gòu)、微型傳感器、微型執(zhí)行器、信號(hào)處理和控制電路、接口、通信、電源等于一體的微型器件或系統(tǒng)，典型器件是陀螺儀、加速度計(jì)和電子羅盤，主要依靠慣性測(cè)量單元進(jìn)行姿態(tài)識(shí)別。采用微機(jī)電系統(tǒng)進(jìn)行姿態(tài)識(shí)別的主要難點(diǎn)在于需通過(guò)算法設(shè)計(jì)，克服傳感器輸出的累積誤差，以達(dá)到較高的識(shí)別精度。1)人體姿態(tài)識(shí)別節(jié)點(diǎn)本專利技術(shù)采用的人體姿態(tài)識(shí)別節(jié)點(diǎn)(以下簡(jiǎn)稱為節(jié)點(diǎn))由傳感器模塊、無(wú)線通信模塊、處理器模塊和電源模塊四個(gè)部分組成。傳感器模塊集成了陀螺儀，型號(hào)為MPU3050；集成了加速度計(jì)和電子羅盤，型號(hào)為L(zhǎng)SM303DLH。這3種傳感器分別負(fù)責(zé)采集角速度、加速度和磁場(chǎng)強(qiáng)度，并將這些傳感器數(shù)據(jù)從物理量轉(zhuǎn)換為電信號(hào)。無(wú)線通信模塊采用nRF24L01無(wú)線收發(fā)器，負(fù)責(zé)在2.4GHz～2.5GHz頻段內(nèi)與基站之間傳輸信息。處理器模塊采用STM32F103嵌入式微處理器，負(fù)責(zé)控制傳感器信號(hào)采集和無(wú)線收發(fā)功能。電源采用額定電壓為3.7V的鋰離子電池，負(fù)責(zé)為節(jié)點(diǎn)提供所需能量，節(jié)點(diǎn)提供鋰離子電池接口。節(jié)點(diǎn)工作時(shí)，將信號(hào)通過(guò)無(wú)線方式傳輸至基站，基站由處理器模塊、無(wú)線通信模塊和串行接口模塊組成，負(fù)責(zé)接收節(jié)點(diǎn)發(fā)送的數(shù)據(jù)，并將接收到的數(shù)據(jù)通過(guò)串行接口傳輸至上位機(jī)。2)四元數(shù)四元數(shù)是一種超復(fù)數(shù)，由一個(gè)標(biāo)量和一個(gè)矢量組成，定義如式(1)所示：其中a、b、c、d、n0為標(biāo)量實(shí)數(shù)，i、j、k為三個(gè)虛部單位，為矢量。四元數(shù)也可表示為向量形式，即Q＝(abcd)T。如果Q滿足a2+b2+c2+d2＝1，則將Q稱作標(biāo)準(zhǔn)化四元數(shù)。兩個(gè)四元數(shù)可以進(jìn)行乘法運(yùn)算。設(shè)Q1＝a+bi+cj+dk，Q2＝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)可通過(guò)式(5)來(lái)描述：式(5)也可通過(guò)矩陣相乘來(lái)表示，如式(6)所示其中矩陣C稱為方向余弦矩陣，是由兩組不同的標(biāo)準(zhǔn)正交基的基底向量之間的方向余弦所形成的矩陣，可用于向量在不同坐標(biāo)系間的坐標(biāo)轉(zhuǎn)換。矩陣C定義如下：若為被動(dòng)旋轉(zhuǎn)，即對(duì)向量所在坐標(biāo)系本身進(jìn)行的逆時(shí)針旋轉(zhuǎn)，則可用式(8)來(lái)描述：標(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ù)對(duì)時(shí)間的導(dǎo)數(shù)與轉(zhuǎn)動(dòng)角速度之間的關(guān)系，即四元數(shù)運(yùn)動(dòng)學(xué)方程：其中，為四元數(shù)對(duì)時(shí)間的導(dǎo)數(shù)，p為載體沿空間三維坐標(biāo)軸轉(zhuǎn)動(dòng)角速度構(gòu)造的四元數(shù)，其實(shí)部為零，即：p＝0+ωxi+ωyj+ωzk(10)為求解包含姿態(tài)信息的四元數(shù)，需對(duì)進(jìn)行積分，然后將其化為姿態(tài)四元數(shù)，這樣便可得到描述從一種姿態(tài)轉(zhuǎn)動(dòng)至另一種姿態(tài)的等價(jià)姿態(tài)四元數(shù)。設(shè)xk＝ak+bki+ckj+dkk為姿態(tài)四元數(shù)，其中k為非負(fù)整數(shù)，表示狀態(tài)序號(hào)。xk用于描述系統(tǒng)在k時(shí)刻的位置狀態(tài)，初始狀態(tài)為x0，其值設(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ù)對(duì)時(shí)間的導(dǎo)數(shù)，定義如式(9)所示，即：將(12)代入(11)得：xk＝xk-1+0.5·xk-1·p·Δt(13)式(13)即為狀態(tài)向量的遞推公式，如果已知x0和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)。對(duì)于多維線性系統(tǒng)，式(15)為系統(tǒng)的狀態(tài)方程，式(16)為輸出方程：xk+1＝Axk+Buk+wk(15)zk＝Hxk+vk(16)其中A、B、H為系數(shù)矩陣，A稱作狀態(tài)轉(zhuǎn)移矩陣，H稱作觀測(cè)矩陣，xk為系統(tǒng)當(dāng)前的狀態(tài)，xk+1為系統(tǒng)下一時(shí)刻的狀態(tài)，uk為系統(tǒng)當(dāng)前的輸入，zk為系統(tǒng)當(dāng)前的輸出，wk為當(dāng)前的過(guò)程噪聲，vk為當(dāng)前的測(cè)量噪聲。式中的下標(biāo)k表示當(dāng)前時(shí)刻，k+1表示下一時(shí)刻。假設(shè)過(guò)程噪聲wk的平均值為零，測(cè)量噪聲vk的平均值也為零，并且wk和vk相互獨(dú)立。過(guò)程噪聲的協(xié)方差S和測(cè)量噪聲的協(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)誤差ek可表示如式(20)所示：先驗(yàn)誤差的協(xié)方差如式(21)所示，即為誤差協(xié)方差的期望值。后驗(yàn)誤差的協(xié)方差如式(22)所示：根據(jù)卡爾曼濾波的基本原理，可由當(dāng)前狀態(tài)的最優(yōu)估計(jì)值推測(cè)下一時(shí)刻狀態(tài)的期望值，然后將系統(tǒng)測(cè)量誤差映射到狀態(tài)域，對(duì)狀態(tài)的期望值進(jìn)行修正，從而得到下一時(shí)刻的最優(yōu)估計(jì)值。按照如上方式系統(tǒng)不斷迭代，從而得到每一個(gè)狀態(tài)的最優(yōu)估計(jì)值。針對(duì)線性系統(tǒng)(15)和(16)的卡爾曼濾波方法由下面5個(gè)經(jīng)典公式表示：Pk'＝APk-1AT+S(24)Kk＝Pk'HT(HPk'HT+R)-1(25)Pk＝Pk'-KkHPk'(27)式(23)表示當(dāng)前時(shí)刻的狀態(tài)期望值可由前一時(shí)刻狀態(tài)及當(dāng)前輸入求得；式(24)表明了當(dāng)前時(shí)刻先驗(yàn)誤差的協(xié)方差的期望值可由前一時(shí)刻的后驗(yàn)誤差協(xié)方差求得；式(25)表明了卡爾曼增益Kk的求解方法；求得卡爾曼增益之后，可通過(guò)式(26)，由當(dāng)前時(shí)刻狀態(tài)期望值和當(dāng)前時(shí)刻觀測(cè)值計(jì)算得到當(dāng)前時(shí)刻狀態(tài)最優(yōu)估計(jì)值；為了使迭代能夠持續(xù)不斷進(jìn)行，還需要通過(guò)式(27)，利用當(dāng)前時(shí)刻先驗(yàn)誤差協(xié)方差計(jì)算求得后驗(yàn)誤差協(xié)方差，將其代入式(24)，即可求得下一時(shí)刻的先驗(yàn)誤差協(xié)方差，從而可以開始下一輪迭代。卡爾曼濾波的5個(gè)經(jīng)典公式中，式(23)和(24)可稱為時(shí)間迭代公式，用于預(yù)測(cè)狀態(tài)值及誤差協(xié)方差；式(25)～(27)為測(cè)量迭代公式，通過(guò)觀測(cè)值修正期望值，從而獲取經(jīng)過(guò)改進(jìn)的期望值，即最優(yōu)估計(jì)值。式(2本文檔來(lái)自技高網(wǎng)...

【技術(shù)保護(hù)點(diǎn)】
一種基于自適應(yīng)擴(kuò)展卡爾曼濾波的人體姿態(tài)識(shí)別方法，其特征在于，本方法基于擴(kuò)展卡爾曼濾波的如下5個(gè)公式設(shè)計(jì)：x^k′=f(x^k-1,uk)+wk---(30)]]>P′k＝ΦPk?1ΦT+S???(31)Kk＝P′kNT(NPk'NT+R)?1???(32)x^k=x^k′+Kk(zk-h(x^k′))---(33)]]>Pk＝P′k?KkNP′k???(34)其中，k表示當(dāng)前時(shí)刻，k?1表示前一時(shí)刻；通過(guò)式(30)由前一時(shí)刻狀態(tài)最優(yōu)估計(jì)值和當(dāng)前時(shí)刻系統(tǒng)輸入計(jì)算當(dāng)前時(shí)刻狀態(tài)期望值，其中，為當(dāng)前時(shí)刻狀態(tài)期望值，xk?1為前一時(shí)刻狀態(tài)最優(yōu)估計(jì)值，uk為系統(tǒng)當(dāng)前時(shí)刻的輸入，wk為當(dāng)前時(shí)刻的過(guò)程噪聲，f(·)是一個(gè)非線性函數(shù)；式(31)為系統(tǒng)誤差迭代公式，其中，Pk'為先驗(yàn)誤差協(xié)方差，Pk?1為后驗(yàn)誤差協(xié)方差，Φ是f(·)的雅克比矩陣，S為過(guò)程噪聲協(xié)方差矩陣；式(32)用于計(jì)算卡爾曼增益，其中，Kk為卡爾曼增益，N為式(33)中h(·)的雅克比矩陣，R為測(cè)量噪聲的協(xié)方差；式(33)為狀態(tài)迭代公式，當(dāng)前狀態(tài)的最優(yōu)估計(jì)值經(jīng)由當(dāng)前狀態(tài)期望值和殘差求得，其中，為系統(tǒng)當(dāng)前時(shí)刻的最優(yōu)狀態(tài)估計(jì)值，zk為系統(tǒng)當(dāng)前輸出，h(·)也是一個(gè)非線性函數(shù)；式(34)為誤差協(xié)方差迭代公式；本方法基于節(jié)點(diǎn)工作，節(jié)點(diǎn)集成了陀螺儀、加速度計(jì)和電子羅盤，分別負(fù)責(zé)采集角速度、加速度和磁場(chǎng)強(qiáng)度傳感器數(shù)據(jù)，并將傳感器數(shù)據(jù)從物理量轉(zhuǎn)換為電信號(hào)；該方法包括模型設(shè)計(jì)和參數(shù)設(shè)計(jì)兩個(gè)部分；(1)模型設(shè)計(jì)用于解決人體姿態(tài)識(shí)別的模型推導(dǎo)問(wèn)題；人體運(yùn)動(dòng)時(shí)肢體的角度由姿態(tài)四元數(shù)來(lái)表示，采用擴(kuò)展卡爾曼濾波方法，通過(guò)四元數(shù)運(yùn)動(dòng)學(xué)方程，根據(jù)傳感器數(shù)據(jù)和當(dāng)前姿態(tài)四元數(shù)，計(jì)算下一時(shí)刻姿態(tài)四元數(shù)的期望值；對(duì)于姿態(tài)四元數(shù)xk＝(ak?bk?ck?dk)T，根據(jù)四元數(shù)乘法的性質(zhì)，將四元數(shù)運(yùn)動(dòng)學(xué)方程的狀態(tài)向量遞推公式表示為式(35)，即節(jié)點(diǎn)轉(zhuǎn)動(dòng)的系統(tǒng)狀態(tài)方程：xk=akbkckdk=1-0.5·Δtωx-0.5·Δtωy-0.5·Δtωz0.5·Δtωx10.5·Δtωz-0.5·Δtωy0.5·Δtωy-0.5·Δtωz10.5·Δtωx0.5·Δtωz0.5·Δtωy-0.5·Δtωx1ak-1bk-1ck-1dk-1=Φxk-1---(35)]]>其中，Δt為從k?1時(shí)刻到k時(shí)刻經(jīng)歷的時(shí)間，ωx、ωy、ωz為空間三個(gè)軸向的轉(zhuǎn)動(dòng)角速度，由陀螺儀采集獲得；由于狀態(tài)方程本身是一個(gè)線性方程，轉(zhuǎn)移矩陣為Φ，如式(36)所示，因此進(jìn)行擴(kuò)展卡爾曼濾波時(shí)不需對(duì)狀態(tài)轉(zhuǎn)移矩陣進(jìn)行泰勒級(jí)數(shù)展開；Φ=1-0.5·Δtωx-0.5·Δtωy-0.5·Δtωz0.5·Δtωx10.5·Δtωz-0.5·Δtωy0.5·Δtωy-0.5·Δtωz10.5·Δtωx0.5·Δtωz0.5·Δtωy-0.5·Δtωx1---(36)]]>假定已知k?1時(shí)刻姿態(tài)四元數(shù)的后驗(yàn)誤差協(xié)方差矩陣Pk?1，k時(shí)刻姿態(tài)四元數(shù)的先驗(yàn)誤差協(xié)方差矩陣Pk'由式(31)求得，如式(37)所示：Pk′=1-0.5Δtωx-0.5Δtωy-0.5Δtωz0.5Δtωx10.5Δtωz-0.5Δtωy0.5Δtωy-0.5Δtωz10.5Δtωx0.5&De...

【技術(shù)特征摘要】
1.一種基于自適應(yīng)擴(kuò)展卡爾曼濾波的人體姿態(tài)識(shí)別方法，其特征在于，本方法基于擴(kuò)展卡爾曼濾波的如下5個(gè)公式設(shè)計(jì)：x^k′=f(x^k-1,uk)+wk---(30)]]>P′k＝ΦPk-1ΦT+S(31)Kk＝P′kNT(NPk'NT+R)-1(32)x^k=x^k′+Kk(zk-h(x^k′))---(33)]]>Pk＝P′k-KkNP′k(34)其中，k表示當(dāng)前時(shí)刻，k-1表示前一時(shí)刻；通過(guò)式(30)由前一時(shí)刻狀態(tài)最優(yōu)估計(jì)值和當(dāng)前時(shí)刻系統(tǒng)輸入計(jì)算當(dāng)前時(shí)刻狀態(tài)期望值，其中，為當(dāng)前時(shí)刻狀態(tài)期望值，xk-1為前一時(shí)刻狀態(tài)最優(yōu)估計(jì)值，uk為系統(tǒng)當(dāng)前時(shí)刻的輸入，wk為當(dāng)前時(shí)刻的過(guò)程噪聲，f(·)是一個(gè)非線性函數(shù)；式(31)為系統(tǒng)誤差迭代公式，其中，Pk'為先驗(yàn)誤差協(xié)方差，Pk-1為后驗(yàn)誤差協(xié)方差，Φ是f(·)的雅克比矩陣，S為過(guò)程噪聲協(xié)方差矩陣；式(32)用于計(jì)算卡爾曼增益，其中，Kk為卡爾曼增益，N為式(33)中h(·)的雅克比矩陣，R為測(cè)量噪聲的協(xié)方差；式(33)為狀態(tài)迭代公式，當(dāng)前狀態(tài)的最優(yōu)估計(jì)值經(jīng)由當(dāng)前狀態(tài)期望值和殘差求得，其中，為系統(tǒng)當(dāng)前時(shí)刻的最優(yōu)狀態(tài)估計(jì)值，zk為系統(tǒng)當(dāng)前輸出，h(·)也是一個(gè)非線性函數(shù)；式(34)為誤差協(xié)方差迭代公式；本方法基于節(jié)點(diǎn)工作，節(jié)點(diǎn)集成了陀螺儀、加速度計(jì)和電子羅盤，分別負(fù)責(zé)采集角速度、加速度和磁場(chǎng)強(qiáng)度傳感器數(shù)據(jù)，并將傳感器數(shù)據(jù)從物理量轉(zhuǎn)換為電信號(hào)；該方法包括模型設(shè)計(jì)和參數(shù)設(shè)計(jì)兩個(gè)部分；(1)模型設(shè)計(jì)用于解決人體姿態(tài)識(shí)別的模型推導(dǎo)問(wèn)題；人體運(yùn)動(dòng)時(shí)肢體的角度由姿態(tài)四元數(shù)來(lái)表示，采用擴(kuò)展卡爾曼濾波方法，通過(guò)四元數(shù)運(yùn)動(dòng)學(xué)方程，根據(jù)傳感器數(shù)據(jù)和當(dāng)前姿態(tài)四元數(shù)，計(jì)算下一時(shí)刻姿態(tài)四元數(shù)的期望值；對(duì)于姿態(tài)四元數(shù)xk＝(akbkckdk)T，根據(jù)四元數(shù)乘法的性質(zhì)，將四元數(shù)運(yùn)動(dòng)學(xué)方程的狀態(tài)向量遞推公式表示為式(35)，即節(jié)點(diǎn)轉(zhuǎn)動(dòng)的系統(tǒng)狀態(tài)方程：xk=akbkckdk=1-0.5·Δtωx-0.5·Δtωy-0.5·Δtωz0.5·Δtωx10.5·Δtωz-0.5·Δtωy0.5·Δtωy-0.5·Δtωz10.5·Δtωx0.5·Δtωz0.5·Δtωy-0.5·Δtωx1ak-1bk-1ck-1dk-1=Φxk-1---(35)]]>其中，Δt為從k-1時(shí)刻到k時(shí)刻經(jīng)歷的時(shí)間，ωx、ωy、ωz為空間三個(gè)軸向的轉(zhuǎn)動(dòng)角速度，由陀螺儀采集獲得；由于狀態(tài)方程本身是一個(gè)線性方程，轉(zhuǎn)移矩陣為Φ，如式(36)所示，因此進(jìn)行擴(kuò)展卡爾曼濾波時(shí)不需對(duì)狀態(tài)轉(zhuǎn)移矩陣進(jìn)行泰勒級(jí)數(shù)展開；Φ=1-0.5·Δtωx-0.5·Δtωy-0.5·Δtωz0.5·Δtωx10.5·Δtωz-0.5·Δtωy0.5·Δtωy-0.5·Δtωz10.5·Δtωx0.5·Δtωz0.5·Δtωy-0.5·Δtωx1---(36)]]>假定已知k-1時(shí)刻姿態(tài)四元數(shù)的后驗(yàn)誤差協(xié)方差矩陣Pk-1，k時(shí)刻姿態(tài)四元數(shù)的先驗(yàn)誤差協(xié)方差矩陣Pk'由式(31)求得，如式(37)所示：Pk′=1-0.5Δtωx-0.5Δtωy-0.5Δtωz0.5Δtωx10.5Δtωz-0.5Δtωy0.5Δtωy-0.5Δtωz10.5Δtωx0.5Δtωz0.5Δtωy-0.5Δtωx1Pk-11-0.5Δtωx-0.5Δtωy-0.5Δtωz-0.5Δtωx1-0.5Δtωz-0.5Δtωy-0.5Δtωy-0.5Δtωz1-0.5Δtωx-0.5Δtωz-0.5Δtωy-0.5Δtωx1+S---(37)]]>采用節(jié)點(diǎn)加速度和磁場(chǎng)強(qiáng)度作為觀測(cè)值，分別對(duì)狀態(tài)期望值進(jìn)行校正；由于狀態(tài)域和觀測(cè)域的數(shù)據(jù)類型不同，通過(guò)觀測(cè)函數(shù)h(·)將當(dāng)前姿態(tài)四元數(shù)轉(zhuǎn)換為載體坐標(biāo)系加速度；設(shè)初始時(shí)刻測(cè)得節(jié)點(diǎn)的加速度值對(duì)向量所在坐標(biāo)系本身進(jìn)行逆時(shí)針旋轉(zhuǎn)，用當(dāng)前姿態(tài)四元數(shù)(a,b,c,d)變換得到載體坐標(biāo)系的加速度如式(38)所示：a→′=CTa→=a2+b2-c2-d22(bc+ad)2(bd-ac)2(bc-ad)a2-b2+c2-d22(cd+ab)2(bd+ac)2(cd-ab)a2-b2-c2+d2axayaz---(38)]]>通過(guò)矩陣乘法得出觀測(cè)函數(shù)h(·)中各分量的表達(dá)式，如式(39)所示：ax′=(a2+b2-c2-d2)ax+2(bc+ad)ay+2(bd-ac)azay′=2(bc-ac)ax+(a2-b2+c2-d2)ay+2(cd+ab)azaz′=2(bd+ac)ax+2(cd-ab)ay+(a2-b2-c2+d2)az---(39)]]>按照式(40)求函數(shù)h(·)的雅克比矩陣，即令h(·)的各分量對(duì)四元數(shù)(a,b,c,d)的各項(xiàng)分別求偏導(dǎo)，得式(41)：J=∂a→x′∂a∂a→x′∂b∂a→x′∂c∂a→x′∂d∂a→y′∂a∂a→y′∂b∂a→y′∂c∂a→y′∂d∂a→z′∂a∂a→z′∂b∂a→z′∂c∂a→z′∂d---(40)]]>J=2×a·ax-c·az+d·ayb·ax+...

【專利技術(shù)屬性】
技術(shù)研發(fā)人員：賴曉晨，遲宗正，史文哲，劉鑫，
申請(qǐng)(專利權(quán))人：大連理工大學(xué)，
類型：發(fā)明
國(guó)別省市：遼寧;21