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

如果了解动画制作或者游戏制作的读者应该比较清楚在建立三维模型时是可以加如骨骼控制，还可以加入肌肉效果，这样可以高度还原人物动作，甚至外观，就像动画片里面的人物一样可以做出各种动作，把捕获到的姿态信息输入给模型，模型就可以实现动作还原。用摄像头等光学传感器来实现动作捕捉的方案也比较成熟了，优点是可以精确捕捉到动作，光线环境要求高、投入资金大的缺点也很明显，后来就有一些人就开始使用惯性传感器来实现动作捕获，本文是以这个视频为最终目标。 大致实现3维模型还原人体动作，会从2D到3维逐步实现，实验主要用到以下3个环节：

• 3轴加速度+3轴角速度+3轴磁力计（用来检测关节姿态，解算还需要一个主控芯片）
• 上位机（绘图还原动作）
• 无线模块（无线传输姿态数据，有线也行）

传感器

9轴传感器部分首先要做的就是姿态角解算，这一部分不是本文的重点，特别是使用STM32作为主控的网上也能找到比较完整的例子，也就不再做过多描述，最终目的就是输出3轴的角度。如果不用磁力计，短时间内的航向角漂移不影响实验效果，开源的硬件模块也很多，笔者就是使用的现成CC3D飞控：

二维表示

传感器摆放位置大致如下，具体传感器的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轴加速度分别对时间二重积分即可