9轴传感器首先要做的是姿态角解算，这不是本文的重点，尤其是使用STM32也可以在网上找到一个比较完整的例子作为主控，所以不再做太多描述，最终目的是输出3轴的角度。如果不使用磁力计，短时间内的航向角漂移不会影响实验效果，开源硬件模块也很多，作者使用了现成的CC3D飞行控制，然后改装添加蓝牙模块，蓝牙模块也在网上购买CC2640，因为只有5套，大腿和小腿的关节没有区别:

在上面的动画把手Android手机在电脑上投屏，使用模型Maya几个简单拖出的长方体和球，Android应用是使用opengl把Maya导出的模型画在屏幕上，在这里提供基础Android显示3D模型的demo。当然，也可以直接一个桌面应用，作者也可以Qt使用Android的好处是，手机可以同时连接5颗蓝牙，连接更方便。如果计算机端需要蓝牙模块转动USB，一个模块只能连接三个传感器，也就是说，需要在计算机上插入两颗蓝牙来连接五个传感器，连接过程相对冗长，需要根据模块指令发送，不支持线下连接（理论上可以实现一个二次开发CC2640连接8个从机蓝牙，实际效果可能不理想，不需要时间研究): 所以最后，我又写了一个版本Android的：

传感器的位置大致如下，具体传感器X,Y,Z方向可以与图中不同。关键是要知道身体动作影响传感器的角度、角度值和±符号会影响计算结果：

可使用上位机Qt或者C#找个控件画折线图，画出的身体结构如下(4点连3条线): 侧面面向屏幕右侧，做了一些弯腰、抬腿、走路的动作: 上图中每个关节的长度固定len=4(值没有意义，只表示长度符号)，从上到下三段关节，传感器绕Z轴旋转angle1，angle2，angle3，三个角度是0 ，四个圆环就是各个关节的两端，从上到下坐标点依次为P1，P2，P3，P4，以身体上端为圆点P1=(0, 0)，身体下端P可以通过2的坐标angle1和len根据计算，身体的下端是大腿的上端，大腿的下端P3可以由P2、angle2和len根据计算，大腿下端P4又可以由P3、angle3和len计算得出，几个坐标点分别为如下计算方式得出：

P 1 = ( 0 , 0 ) P1=(0, 0) P1=(0,0) P 2 = ( s i n ( a n g l e 1 ) ? l e n ， c o s ( a n g l e 1 ) ? l e n ) P2 =(sin(angle1)*len，cos(angle1)*len) P2=(sin(angle1)∗len，cos(angle1)∗len) P 3 = ( P 2. x + s i n ( a n g l e 2 ) ∗ l e n ， P 2. y + c o s ( a n g l e 2 ) ∗ l e n ) P3 =(P2.x+sin(angle2)*len，P2.y+cos(angle2)*len) P3=(P2.x+sin(angle2)∗len，P2.y+cos(angle2)∗len) P 4 = ( P 3. x + s i n ( a n g l e 3 ) ∗ l e n ， P 3. y + c o s ( a n g l e 3 ) ∗ l e n ) P4 =(P3.x+sin(angle3)*len，P3.y+cos(angle3)*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(sizeof(float)*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’就是身体的下端也就是大腿的上端，和上面二维的思想一样，可由如下思路得到新的坐标点：

P 1 = ( 0 , 0 , 0 ) P1=(0, 0, 0) P1=(0,0,0) P 2 = r o t a t e ( r o l l 1 , p i t c h 1 , y a w 1 , P 1. x , P 1. y , P 1. z + l e n ) P2 =rotate(roll1,pitch1, yaw1, P1.x, P1.y, P1.z+len) P2=rotate(roll1,pitch1,yaw1,P1.x,P1.y,P1.z+len) P 3 = r o t a t e ( r o l l 2 , p i t c h 2 , y a w 2 , P 2. x , P 2. y , P 2. z + l e n ) P3 =rotate(roll2, pitch2, yaw2, P2.x, P2.y, P2.z+len) P3=rotate(roll2,pitch2,yaw2,P2.x,P2.y,P2.z+len) P 4 = r o t a t e ( r o l l 3 , p i t c h 3 , y a w 3 , P 3. x , P 3. y , P 3. z + l e n ) P4 =rotate(roll3, pitch3, yaw3, P3.x, P3.y, P3.z+len) P4=rotate(roll3,pitch3,yaw3,P3.x,P3.y,P3.z+len) 最后由三维绘图连接起来就能得到3维的姿态还原效果。传感器越多，把人体关节分的越细，画出来的3维模型就跟人体越接近。本文中只叙述了大致思路，并未对过程中使用的角度、方向做仔细描述，如果读者自己动手做，在了解姿态解算之后应该自然能明白方向关系。

要知道只从加速度传感器上计算位置信息是不准确度，随着时间的延长位置信息误差会越来越明显，计算结果只能在短时间内参考，大致计算步骤如下：

1. 将3轴加速度使用旋转矩阵转到水平方向和竖直方向
2. 将旋转后的的3轴加速度分别对时间二重积分即可

最后附上一个网上看到的效果，如果有人知道视频中用的模型软件是什么可以留言告诉我，可以找来学习一下，用这种3维软件的好处就是骨骼绑定可以在软件中事先设置好，只需要传角度过去就好，不需要像本文一样做位置转换。