一種基於四元數的下肢姿態檢測方法及系統
2024-04-16 15:20:05
1.本發明涉及康復訓練技術領域，尤其涉及一種基於四元數的下肢姿態檢測方法及系統。
背景技術：
2.本部分的陳述僅僅是提供了與本發明相關的背景技術信息，不必然構成在先技術。
3.大量腦癱兒童不能實現正常獨立行走，最終只能以異常的步態踉蹌行走；可穿戴式兒童下肢康復訓練器械能夠幫助實現腦癱患兒更好的行走，目前已經得到了廣泛的應用。可穿戴式兒童下肢康復訓練器械以骨骼式機械腿支撐並帶動患兒下肢進行步行訓練，然後通過檢測下肢動作數據，對患者的訓練情況進行評估，以確定訓練效果。
4.因此，對於下肢姿態數據的準確檢測是對訓練情況進行有效評估的前提；現有技術中，往往採用姿態濾波的方式進行姿態計算，捕獲下肢動作數據，然後進行動作評估；傳統的濾波方法是藉助加速度計、陀螺儀、磁力計的輸出，構造姿態狀態和觀測方程，對姿態進行濾波的過程，例如卡爾曼濾波。因為加速度/磁力計具有高頻噪聲，將加速度/磁力計的信號看成是音頻信號，它們的信號會有很多「毛刺」，也就是說它們的瞬時值不夠精確，解算出來的姿態會震蕩。而陀螺儀具有低頻噪聲，即每個時刻的得到的角速度是比較精確的，使用積分就能得到旋轉姿態，但是積分會累積誤差，因此積分到後面姿態就不對了，也就是漂移現象。
5.為了克服噪聲帶來的誤差，現有技術使用基於歐拉角的卡爾曼濾波器進行腿部動作捕捉，但是，當捕捉節點的俯仰角為90度的時候就會產生萬向節死鎖現象，導致捕捉動作異常；而且歐拉角法計算量大，在計算時需要大量的矩陣求逆運算，導致系統的動態響應變差。
技術實現要素：
6.為了解決上述問題，本發明提出了一種基於四元數的下肢姿態檢測方法及系統，該方法使得姿態描述方式的物理意義更為直接，姿態檢測結果更加準確。
7.在一些實施方式中，採用如下技術方案：
8.一種基於四元數的下肢姿態檢測方法，包括：
9.建立以姿態四元數為狀態變量的狀態方程，建立以膝關節與髖關節的位置坐標差、踝關節與髖關節的位置坐標差為觀測量的觀測方程；
10.獲取人體下肢在初始時刻的姿態四元數，基於人體下肢在n-1時刻的姿態四元數和角速度，一步預測其在n時刻的姿態四元數；
11.基於狀態轉移矩陣對狀態量的誤差協方差進行預測；基於預測的誤差協方差和測量矩陣構建卡爾曼增益方程，計算卡爾曼增益；
12.基於一步預測的姿態四元數、卡爾曼增益、觀測量的測量值與估計值，更新估計姿
態四元數，得到下肢姿態數據；
13.更新誤差協方差，進行下一時刻姿態四元數的估計。
14.作為進一步地方案，所述狀態方程和觀測方程分別為：
[0015][0016]
其中，和分別表示n時刻和n-1時刻下大腿和小腿的姿態四元數；ω1、ω2分別為大腿和小腿傳感器輸出角速的擴展矩陣；t表示為傳感器的採樣周期；lp、ld分別表示大腿和小腿的長度；表示膝蓋坐標系k繫到導航坐標系n系的旋轉矩陣的第一列，表示腳踝坐標系a繫到導航坐標系n系的旋轉矩陣的第一列，表示由參考坐標系c繫到導航坐標系n系的旋轉矩陣。
[0017]
作為進一步地方案，基於人體下肢在n-1時刻的姿態四元數和角速度，一步預測其在n時刻的姿態四元數，具體為：
[0018][0019]
其中，f
n-1
為狀態轉移矩陣，為n-1時刻下大腿和小腿的姿態四元數。
[0020]
作為進一步地方案，基於狀態轉移矩陣對狀態變量的誤差協方差進行預測，具體為：
[0021]
p n|n-1
＝f
n-1
p
n-1fn-1t
+q
[0022]
其中，p
n|n-1
為預測的n時刻的誤差協方差矩陣，f
n-1
為狀態轉移矩陣，p
n-1
為n-1時刻的誤差協方差矩陣，q為處理噪聲矩陣。
[0023]
作為進一步地方案，基於預測的誤差協方差和測量矩陣構建卡爾曼增益方程，計算卡爾曼增益，具體為：
[0024]kn
＝p
n|n-1hnt
(hnp
n|n-1hnt
+r)
[0025]
其中，kn為n時刻的卡爾曼增益，p
n|n-1
為預測的n時刻的誤差協方差矩陣，hn為測量矩陣，其含義為估計的觀測量對一步預測的狀態量求偏導的雅可比矩陣，r為測量噪聲矩陣。
[0026]
作為進一步地方案，所述更新估計姿態四元數，具體為：
[0027][0028]
其中，為一步預測的n時刻的姿態四元數，kn為n時刻的卡爾曼增益，zn為n時刻觀測量的測量值，hn為由狀態變量的一步預測表示的估計觀測量。
[0029]
作為進一步地方案，所述更新誤差協方差，具體為：
[0030]
pn＝(i8×
8-k
nhn
)p
n|n-1
[0031]
其中，p
n|n-1
為預測誤差協方差，kn為n時刻的卡爾曼增益，hn為測量矩陣。
[0032]
在另一些實施方式中，採用如下技術方案：
[0033]
一種基於四元數的下肢姿態檢測系統，包括：
[0034]
方程構建模塊，用於建立以姿態四元數為狀態變量的狀態方程，建立以膝關節與髖關節的位置坐標差、踝關節與髖關節的位置坐標差為觀測量的觀測方程；
[0035]
一步預測模塊，用於獲取人體下肢在初始時刻的姿態四元數，基於人體下肢在n-1時刻的姿態四元數和角速度，一步預測其在n時刻的姿態四元數；
[0036]
姿態預測模塊，用於基於狀態轉移矩陣對狀態量的誤差協方差進行預測；基於預測的誤差協方差和測量矩陣構建卡爾曼增益方程，計算卡爾曼增益；基於一步預測的姿態四元數、卡爾曼增益、觀測量的測量值與估計值，更新估計姿態四元數，得到下肢姿態數據；
[0037]
誤差協方差更新模塊，用於更新誤差協方差，進行下一時刻姿態四元數的估計。
[0038]
在另一些實施方式中，採用如下技術方案：
[0039]
一種終端設備，其包括處理器和存儲器，處理器用於實現各指令；存儲器用於存儲多條指令，所述指令適於由處理器加載並執行上述的基於四元數的下肢姿態檢測方法。
[0040]
在另一些實施方式中，採用如下技術方案：
[0041]
一種計算機可讀存儲介質，其中存儲有多條指令，所述指令適於由終端設備的處理器加載並執行上述的基於四元數的下肢姿態檢測方法。
[0042]
與現有技術相比，本發明的有益效果是：
[0043]
(1)本發明基於四元數的下肢姿態檢測方法，計算量小，便於計算機或控制器實時處理大量的運算，能夠保證系統的實時性。
[0044]
(2)相比於現有技術中的歐拉角與方向餘弦的方法，本發明使用基於四元數的姿態濾波算法，能夠有效避免萬向節死鎖的現象，計算量小，準確率高。能夠準確預估患兒下肢姿態數據，通過與標準訓練資料庫中的標準數據進行比較，可以準確了解患兒康復訓練的質量，為醫生制定下一步的康復訓練計劃提供數據支撐。
[0045]
本發明的其他特徵和附加方面的優點將在下面的描述中部分給出，部分將從下面的描述中變得明顯，或通過本方面的實踐了解到。
附圖說明
[0046]
圖1為本發明實施例中的基於四元數的下肢姿態檢測方法流程圖；
[0047]
圖2為本發明實施例中的傳感器安裝位置及坐標系示意圖；
[0048]
圖3為本發明實施例中的參考點和關節點的位置及坐標系示意圖。
具體實施方式
[0049]
應該指出，以下詳細說明都是例示性的，旨在對本技術提供進一步的說明。除非另有指明，本發明使用的所有技術和科學術語具有與本技術所屬技術領域的普通技術人員通常理解的相同含義。
[0050]
需要注意的是，這裡所使用的術語僅是為了描述具體實施方式，而非意圖限制根據本技術的示例性實施方式。如在這裡所使用的，除非上下文另外明確指出，否則單數形式也意圖包括複數形式，此外，還應當理解的是，當在本說明書中使用術語「包含」和/或「包括」時，其指明存在特徵、步驟、操作、器件、組件和/或它們的組合。
[0051]
實施例一
[0052]
在一個或多個實施方式中，公開了一種基於四元數的下肢姿態檢測方法，該方法能夠基於上一時刻姿態預估當前的姿態，並可以完成對下肢關節進行測量，得到測量姿態；然後結合前期對當前的姿態的預估和測量，構建卡爾曼增益，利用卡爾曼增益對當前姿態進行二次估計，得到濾波後的姿態四元數；本實施例基於四元數的下肢姿態檢測方法可以實現下肢功能損傷患兒動作姿態的精準預估，將預估得到的下肢姿態與標準訓練庫中的標準姿態進行比對，從而為醫生準確了解患者當前的訓練質量，制定下一步的訓練計劃提供數據支撐。
[0053]
本實施例中，用於人體下肢姿態捕獲的傳感器器件為xsensdot，xsensdot體積小，重量輕，便於攜帶，並且設置快速，簡單。將傳感器分別置於胸部、左右大腿和左右小腿的中間位置，保持設置在大腿和小腿的兩個傳感器平行，x軸方向為平行於肢體方向向下，在上位機的控制下，通過藍牙進行數據傳輸，獲得姿態四元數和角速度信息，並以胸部傳感器位置坐標係為參考坐標系。傳感器的安裝位置及其坐標系如圖2所示。除此之外，還需要測量的數據有：髖關節在參考系下的坐標和肢體長度。
[0054]
結合圖1，本實施例方法具體包括如下過程：
[0055]
(1)建立以姿態四元數為狀態變量的狀態方程，建立以膝關節與髖關節的位置坐標差、踝關節與髖關節的位置坐標差為觀測量的觀測方程；
[0056]
四元數，顧名思義，就是由4個元構成的數：
[0057]
q(q0,q1,q2,q3)＝q0+q1i+q2j+q3k
[0058]
其中q0,q1,q2,q3是實數，i，j，k既是相互正交的單位向量，又是虛數單位
[0059]
四元數有多種表達方式，其中常用的有：
[0060]
複數式：q＝q0+q1i+q2j+q3k,可視為一個超複數。
[0061]
q的共軛複數記為：q
*
＝q
0-q1i-q2j-q3k；
[0062]
三角式：式中，θ為實數，u為單位向量。
[0063]
矩陣式：
[0064]
已知四元數的微分方程：
[0065][0066]
其中表示載體坐標系下的角速度，即陀螺儀輸出值。將微分方程可寫為：
[0067][0068]
由運動學原理可根據n-1時刻的狀態推出n時刻的狀態值qn,即：
[0069][0070]
其中，t表示為傳感器的採樣周期。
[0071]
本實施例中，可以用姿態四元數推導出關節點在參考坐標系下的空間位置坐標，其推導過程如下：
[0072]
四元數的大小用範數表示：若||q||＝1，則q稱為規範化四元數。
[0073]
四元數描述了剛體的定點轉動，即當只關心b系相對於n系的角位置時，可認為b系是由n系經過無中間的一次性等效旋轉形成的。q包含了這種等效旋轉的全部信息：u為旋轉軸和旋轉方向，θ為轉過的角度。由四元數可確定出由n系至b系姿態旋轉矩陣：
[0074][0075]
由於因此：
[0076][0077]
若||q||＝1，則旋轉矩陣表示為：
[0078][0079]
解算出下肢肢體運動過程中的姿態變化後，便可根據肢體尺寸以及參考點的相對位置計算出膝關節和踝關節的坐標。具體解算過程如下：
[0080]
首先確定參考係為c系，解算出的關節坐標都是在此參考系下的位置坐標。以右腿關節為例。設右髖關節坐標系與參考系方向保持一致，膝關節和踝關節坐標系與安裝在大腿和小腿上的傳感器坐標方向保持一致。lp為大腿長度；ld為小腿長度。
[0081]
則右膝關節坐標系(k系)到參考坐標系(c系)的旋轉矩陣可表示為：
[0082][0083]
右踝關節(a系)到參考坐標系(c系)的旋轉矩陣可表示為：
[0084][0085]
通過實測，得到由髖關節的坐標為：
[0086]
ph＝(xh,yh,zh)
[0087]
由於膝關節坐標系x軸平行於腿部方向，所以只有在x軸方向上膝關節相對參考點ph有分量，則右膝關節在參考坐標系(c系)的坐標位置為：
[0088][0089]
式中，為坐標轉換矩陣的第一列向量。
[0090]
同理得到右踝關節在參考系(c系)的坐標位置為：
[0091][0092]
式中，為坐標轉換矩陣的第一列向量。
[0093]
以上就是解算下肢關節坐標的步驟，左腿關節坐標解算與右腿方法一致。
[0094]
基於此，建立以姿態四元數為狀態變量的狀態方程；建立以膝蓋、腳踝關節與髖關節位置坐標的差為觀測量的觀測方程；具體為：
[0095][0096]
其中，表示n時刻下大腿和小腿的姿態四元數；ω1、ω2為大腿和小腿角速的擴展矩陣，ω表示為：擴展矩陣，ω表示為：表示載體坐標系下的角速度，即陀螺儀輸出值；t表示為傳感器的採樣周期；觀測量hn表示以膝關節與髖關節的位置坐標差、踝關節與髖關節的位置坐標差，可表示為pk、pa、ph分別表示在參考坐標系下，膝關節和踝關節的位置坐標；lp、ld分別表示大腿和小腿的長度；表示膝蓋坐標系k繫到導航坐標系n系(即東北天坐標系)的旋轉矩陣的第一列，表示腳踝坐標系a繫到導航坐標系n系的旋轉矩陣的第一列，表示由參考坐標系c繫到導航坐標系n系的旋轉矩陣,其表示為：
[0097][0098]
(2)獲取人體下肢在初始時刻的姿態四元數，基於人體下肢在n-1時刻的姿態四元數和角速度，一步預測其在n時刻的姿態四元數；
[0099]
本實施例中，利用n-1時刻的姿態四元數和角速度一步預測姿態四元數時，判斷n-1時刻是否為初始時刻，若是初始時刻，無法直接獲得歷史數據中經過濾波後的姿態四元數，此時會採用傳感器在初始時刻直接測量得到的姿態四元數和初始時刻的角速度值，估計下一時刻的預估姿態；若為初始時刻以外的任意時刻，可基於遞推公式，由初始時刻的姿態四元數和角速度測量值遞推得到n-1時刻(第2個時刻)的姿態四元數，利用n-1時刻的姿態四元數和角速度測量值可得到n時刻的姿態四元數的一步預測。
[0100]
一步預測在n時刻的姿態四元數為：
[0101][0102]
其中，f
n-1
為狀態轉移矩陣，表示為：為狀態轉移矩陣，表示為：表示為對當前時刻大腿和小腿的姿態四元數的一步預測，表示為上一時刻大腿和小腿姿態四元數的濾波值。
[0103]
(3)基於狀態轉移矩陣對狀態量的誤差協方差進行預測；基於預測的誤差協方差和測量矩陣構建卡爾曼增益方程，計算卡爾曼增益；
[0104]
本實施例中，預測誤差協方差採用下式進行表示：
[0105]
p n|n-1
＝f
n-1
p
n-1fn-1t
+q；
[0106]
其中，p
n|n-1
代表n時刻的預測誤差協方差，p
n-1
代表上一時刻誤差協方差的更新值；f
n-1
為狀態方程中的狀態轉移矩陣；q代表處理噪聲矩陣，它描述預測模型帶來的不確定情況，值越大表明預測值越不可靠，算法最後結果更靠近測量值。
[0107]
本實施例中，卡爾曼增益方程的表達式為：
[0108]kn
＝p
n|n-1hnt
(hnp
n|n-1hnt
+r)；
[0109]hn
為測量矩陣，其含義為估計的觀測量對一步預測的狀態量求偏導的雅可比矩陣。r代表測量噪聲矩陣，它描述測量過程中帶來的不確定性，值越大表明測量越不可靠，算法最後越靠近預測值。
[0110]
對n時刻的測量姿態則是真實測量出關節在參考坐標系下的坐標值，參考坐標系，對關節坐標值與固定的髖關節坐標值作差，將差值作為觀測量，即：其中pk為膝關節在參考坐標系下的坐標；pa為踝關節在參考坐標系下的坐標；ph為髖關節在參考坐標系
下的坐標。參考點和關節點的位置及坐標系示意圖如圖3所示。
[0111]
根據觀測量的物理意義，得出對觀測量的估計值，即:
[0112][0113]
其中，表示由膝蓋坐標繫到導航坐標系的旋轉矩陣的第一列；表示為由腳踝坐標繫到導航坐標系的旋轉矩陣的第一列，將h對狀態量求偏導，得到雅可比矩陣h，可表示為：
[0114][0115]
(4)基於一步預測的姿態四元數、卡爾曼增益、觀測量的測量值與估計值，更新估計姿態四元數，得到下肢姿態數據，比如：關節點在參考坐標系下的空間位置坐標。
[0116]
本實施例中，更新估計狀態值姿態四元數的表達式為：
[0117][0118]
其中，zn表示n時刻觀測量的測量值，即的測量值，hn表示n時刻由狀態量的一步預測表示的估計觀測量，即
[0119]
(5)更新誤差協方差，進行下一時刻姿態四元數的估計。
[0120]
本實施例中,更新誤差協方差的表達式為：
[0121]
pn＝(i8×
8-k
nhn
)p
n|n-1
；
[0122]
其中，p
n|n-1
為預測誤差協方差。完成誤差協方差的更新之後，完成了對狀態量的濾波處理，進入下一循環。
[0123]
本實施例方法可實現基於四元數的人體下肢姿態濾波處理，可有效用於人體下肢姿態的濾波解算；相較於現有技術中利用加速度計、磁強計解算方法，該方法使其描述的物理意義明確，並且計算量小，能夠很好地抑制震蕩。
[0124]
實施例二
[0125]
在一個或多個實施方式中，公開了一種基於四元數的下肢姿態檢測系統，包括：
[0126]
方程構建模塊，用於建立以姿態四元數為狀態變量的狀態方程，建立以膝關節與髖關節的位置坐標差、踝關節與髖關節的位置坐標差為觀測量的觀測方程；
[0127]
一步預測模塊，用於獲取人體下肢在初始時刻的姿態四元數，基於人體下肢在n-1時刻的姿態四元數和角速度，一步預測其在n時刻的姿態四元數；
[0128]
姿態預測模塊，用於基於狀態轉移矩陣對狀態量的誤差協方差進行預測；基於預測的誤差協方差和測量矩陣構建卡爾曼增益方程，計算卡爾曼增益；基於一步預測的姿態四元數、卡爾曼增益、觀測量的測量值與估計值，更新估計姿態四元數，得到下肢姿態數據。
[0129]
誤差協方差更新模塊，用於更新誤差協方差，進行下一時刻姿態四元數的估計。
[0130]
需要說明的是，上述各模塊的具體實現方式與實施例一中相同，此處不再詳述。
[0131]
實施例三
[0132]
在一個或多個實施方式中，公開了一種終端設備，包括伺服器，所述伺服器包括存儲器、處理器及存儲在存儲器上並可在處理器上運行的電腦程式，所述處理器執行所述程序時實現實施例一中的基於四元數的下肢姿態檢測方法。為了簡潔，在此不再贅述。
[0133]
應理解，本實施例中，處理器可以是中央處理單元cpu，處理器還可以是其他通用處理器、數位訊號處理器dsp、專用集成電路asic，現成可編程門陣列fpga或者其他可編程邏輯器件、分立門或者電晶體邏輯器件、分立硬體組件等。通用處理器可以是微處理器或者該處理器也可以是任何常規的處理器等。
[0134]
存儲器可以包括只讀存儲器和隨機存取存儲器，並向處理器提供指令和數據、存儲器的一部分還可以包括非易失性隨機存儲器。例如，存儲器還可以存儲設備類型的信息。
[0135]
在實現過程中，上述方法的各步驟可以通過處理器中的硬體的集成邏輯電路或者軟體形式的指令完成。
[0136]
實施例四
[0137]
在一個或多個實施方式中，公開了一種計算機可讀存儲介質，其中存儲有多條指令，所述指令適於由終端設備的處理器加載並執行實施例一中所述的基於四元數的下肢姿態檢測方法。
[0138]
上述雖然結合附圖對本發明的具體實施方式進行了描述，但並非對本發明保護範圍的限制，所屬領域技術人員應該明白，在本發明的技術方案的基礎上，本領域技術人員不需要付出創造性勞動即可做出的各種修改或變形仍在本發明的保護範圍以內。