慣性傳感器實現全身姿態檢測

先看效果：

用到了哪些東西：

傳感器

9軸傳感器部分首先要做的就是姿態角解算，這一部分不是本文的重點，特別是使用STM32作爲主控的網上也能找到比較完整的例子，也就不再做過多描述，最終目的就是輸出3軸的角度。如果不用磁力計，短時間內的航向角漂移不影響實驗效果，開源的硬件模塊也很多，筆者就是使用的現成CC3D飛控，然後自己改裝添加了一個藍牙模塊在裏面，藍牙模塊也是網上買的CC2640，因爲只有5套，大腿小腿這些關節也就沒有區分了：

上位機

上面動畫裏面是把Android手機投屏到電腦上，模型是使用Maya簡單拖出來的幾個長方體和球，Android應用是使用opengl把Maya導出來的模型畫到屏幕上，這裏提供一個基礎的Android顯示3D模型的demo。當然直接編寫一個桌面應用也是可以的，筆者也用Qt遍寫了一套，使用安卓的好處是一個手機可以同時連接5個藍牙，連接比較方便。電腦端的話需要藍牙模塊轉USB，使用串口發指令來連接身上的傳感器，一個模塊還只能連接3個傳感器，也就是說電腦上需要插2個藍牙才能把5個傳感器連上，連接過程也比較囉嗦，需要按模塊指令來發而且不支持掉線重連（二次開發理論上可以實現1個CC2640連接8個從機藍牙，實際效果可能不理想也就不花時間研究了）:
所以最後還是重新寫了一版Android的：

如果瞭解動畫製作或者遊戲製作的讀者應該比較清楚在建立三維模型時是可以加如骨骼控制，還可以加入肌肉效果，這樣可以高度還原人物動作，甚至外觀，就像動畫片裏面的人物一樣可以做出各種動作，把捕獲到的姿態信息輸入給模型，模型就可以實現動作還原。用攝像頭等光學傳感器來實現動作捕捉的方案也比較成熟了，優點是可以精確捕捉到動作，光線環境要求高、投入資金大的缺點也很明顯，後來就有一些人就開始使用慣性傳感器來實現動作捕獲，這個視頻也是使用慣性傳感器實現。 雖然上面動畫那樣大致實現3維模型還原人體動作，但是離實際應用還有一段距離，下面簡單介紹一下筆者實現方式中最關鍵問題，那就是關節位置轉換：

二維表示

傳感器擺放位置大致如下，具體傳感器的X,Y,Z方向可以與圖中有區別，關鍵的是知道肢體動作影響的是傳感器哪個角度，角度值和±符號會影響計算結果：

上位機可以使用Qt或者C#之類的找個繪製折線圖的控件，繪製的身體結構如下(4個點連起來3條線)：

側身面向屏幕右邊，做了一些彎腰，擡腿，走路的動作：

上面圖中每個關節長度就固定爲len=4（數值沒什麼意義，只是表示一個長度符號），從上到下三段關節，傳感器繞Z軸旋轉的角度分別爲angle1，angle2，angle3，立正姿態時三個角度爲0 ，四個圓環就是各個關節的兩端，從上到下座標點依次爲P1，P2，P3，P4，以身體上端爲圓點P1=(0, 0)，身體下端P2的座標就可以由angle1和len計算得出，身體下端就是大腿的上端，大腿的下端P3可以由P2、angle2和len計算得出，大腿的下端P4又可以由P3、angle3和len計算得出，幾個座標點分別爲如下計算方式得出：

$P1=(0, 0)$
$P2 =(sin(angle1)*len，cos(angle1)*len)$
$P3 =(P2.x+sin(angle2)*len，P2.y+cos(angle2)*len)$
$P4 =(P3.x+sin(angle3)*len，P3.y+cos(angle3)*len)$

得出幾個座標點後依次用線連接起來就形成了上圖中的效果，把人體身上的角度實時傳輸到上位機通過這個計算方式計算出來幾個座標點，每計算一次重新畫一遍圖就可以得到上面動畫的效果。由這個方式可以拓展更多的關節。

三維表示

平面的計算相對簡單，那在3維座標系中如何來計算這幾個點？還是上面的這幾個關節，每個傳感器有三個姿態角分別爲pitch(俯仰角),roll(橫滾角),yaw(航向角)，從上到下座標點P1，P2，P3，P4也都變成3維座標點，比如身體上端爲圓點P1=(0, 0，0)，那麼P2應該如何計算呢？這就可以通過旋轉矩陣來實現座標的轉換（可以通過三角函數來推導，這裏也就不細說了），在《慣性導航》一書中的第九章對矩陣也有介紹，值得注意的是旋轉矩陣具有方向性：

矩陣中的符號表示如下：
$\theta$：俯仰角pitch
$\gamma$：橫滾角roll
$\psi$：航向角yaw
通過如下計算就可以得出旋轉後的座標值：

// roll,pitch,yaw分別對應三個軸角度，x,y,z表示三個角度都爲0時對應點的座標
float *rotate(float roll,float pitch,float yaw,float x,float y,float z)
{
	float *out;
	out = (float *)malloc(3);
	float matrix[3][3];
    matrix[0][0] = cos(roll)*cos(yaw)+sin(roll)*sin(yaw)*sin(pitch);
    matrix[0][1] = -cos(roll)*sin(yaw)+sin(roll)*cos(yaw)*sin(pitch);
    matrix[0][2] = -sin(roll)*cos(pitch);
    matrix[1][0] = sin(yaw)*cos(pitch);
    matrix[1][1] = cos(yaw)*cos(pitch);
    matrix[1][2] = sin(pitch);
    matrix[2][0] = sin(roll)*cos(yaw)-cos(roll)*sin(yaw)*sin(pitch);
    matrix[2][1] = -sin(roll)*sin(yaw)-cos(roll)*cos(yaw)*sin(pitch);
    matrix[2][2] = cos(roll)*cos(pitch);

    out[0] = x*matrix[0][0]+y*matrix[0][1]+z*matrix[0][2];
    out[1] = x*matrix[1][0]+y*matrix[1][1]+z*matrix[1][2];
    out[2] = x*matrix[2][0]+y*matrix[2][1]+z*matrix[2][2];
    return out;
}

比如身體上的傳感器三個角度都是0的時候，身體一段因該就是沿Z軸豎直向下：

將身體傳感器3個軸角度和P1初始座標帶入上面的矩陣運算得到P1’=(out[0] ,out[1] ,out[2])就是P1旋轉後的座標值，那麼P1’就是身體的下端也就是大腿的上端，和上面二維的思想一樣，可由如下思路得到新的座標點：

$P1=(0, 0, 0)$
$P2 =rotate(roll1,pitch1, yaw1, P1.x, P1.y, P1.z+len)$
$P3 =rotate(roll2, pitch2, yaw2, P2.x, P2.y, P2.z+len)$
$P4 =rotate(roll3, pitch3, yaw3, P3.x, P3.y, P3.z+len)$
最後由三維繪圖連接起來就能得到3維的姿態還原效果。傳感器越多，把人體關節分的越細，畫出來的3維模型就跟人體越接近。本文中只敘述了大致思路，並未對過程中使用的角度、方向做仔細描述，如果讀者自己動手做，在瞭解姿態解算之後應該自然能明白方向關係。

位置變化

要知道只從加速度傳感器上計算位置信息是不準確度，隨着時間的延長位置信息誤差會越來越明顯，計算結果只能在短時間內參考，大致計算步驟如下：

1. 將3軸加速度使用旋轉矩陣轉到水平方向和豎直方向
2. 將旋轉後的的3軸加速度分別對時間二重積分即可