基于MEMS惯性地磁测量组件的人体运动跟踪与捕捉方法(10)
3.2.1主程序设计
主程序设计为MATLAB中的命令文件，各个子函数均为函数文件。主函数采用Wlile循环结构，设置循环条件为时间t、采样周期为T，当t<t_stop时，程序进入循环体内执行。程序从t=0开始到t=t_stop结束，在循环体内调用各个子函数。子函数包括：四元数算法atti_cal_cq_mod.m、速度计算程序velo_cal,m、位置计算程序posi_cal.m、FQA算法FQA.m。主程序流程图如下
图3.1 主程序流程图
主程序从t=0开始执行，采样周期为T=0.01，循环一次t+T,到t=t_stop结束。在循环体内，先读入数据根据所读数据计算加速度矢量和Energy、加速度和Sum、角速率矢量和Energy2。若满足判断条件则顺序调用FQA算法函数、计算载体速度函数和计算载体位置函数，同时t+T，再判断t是否小于t_stop。若不满足判断条件则顺序调用四元数算法函数、计算载体速度函数和计算载体位置函数。四元数算法的初始姿态利用前一时刻FQA算法解算的姿态信息。注意，如若人体站在原地，只有肢体的运动，则在计算过程中无需计算载体速度和位置。
3.2.2四元数算法软件设计
四元数算法函数基于四元数法计算载体姿态的子函数文件。其输入值为载体陀螺仪测量值Wibb、前一时刻的载体姿态矩阵attiN、载体速度矩阵veloN、载体位置矩阵posiN、周期T。函数输出值为载体姿态矩阵attiN。程序流程图如下（图中Wnbb为载体坐标系相对于地理坐标系的角速度在载体坐标系上的投影）：
图3.2 四元数算法程序流程图
如图所示，四元数算法是一个迭代过程，先由陀螺仪输出的角速率Wibb解算出载体坐标系相对于地理坐标系的角速度在载体坐标系上的投影Wnbb，此过程要用到前一时刻的姿态矩阵。然后再经过计算和等效矢量修正计算出此时刻的姿态四元数从而得到姿态矩阵，从中提取姿态角信息。
3.2.3 FQA算法软件设计
FQA算法函数是用FQA算法计算载体姿态的子函数文件。其输入值为三轴加速度值 和载体系下的地磁信号Mag_B。函数输出值为载体姿态矩阵 和载体姿态四元数 _ 。程序流程图如下：
图3.3 FQA算法程序流程图
由图所示，根据输入的加速度值先计算俯仰角与橫滚角，然后根据磁传感器计算航向角。由流程图就可看出FQA算法的一个显著的优点就是计算量相对较小。
3.2 本章小结
本章首先简要介绍MATLAB软件及其特点。然后主要介绍在上一章的理论基础之上姿态捕捉方法的软件实现。分别说明主程序的工作过程,以及主程序中调用的各个子函数的工作工程，并附有主文件、四元数算法及FQA算法的程序流程图。