惯性传感器实现全身姿态检测

先看效果：

用到了哪些东西：

传感器

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)\ast len，cos(angle1)\ast len)$
$P3=(P2.x+sin(angle2)\ast len，P2.y+cos(angle2)\ast len)$
$P4=(P3.x+sin(angle3)\ast len，P3.y+cos(angle3)\ast 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轴加速度分别对时间二重积分即可

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