本实用新型属于模式识别和智能控制领域，涉及一种脚-机跟随控制系统，尤其涉及一种基于惯性测量单元的脚-机跟随控制系统。
背景技术：
脚-机跟随控制，即用人脚去控制机械臂等执行器，使得人脚的运动能映射到机械臂中，机械臂能够跟随人脚的运动，属于人机交互的一种。随着微电子技术和传感器技术的发展以及虚拟现实技术的兴起，脚-机跟随控制的应用也越来越多。
已有的脚-机跟随系统包括如下几类：(1)操纵杆，操作困难，由于脚的灵活度不如手，即使对于健全的人，操纵杆的控制也比较困难；(2)踏板，功能有限，比如汽车的刹车，只有踩下和松开两种状态，往往还需要额外的机械结构；(3)游戏应用中的跳舞毯(脚踏键盘)，要求人脚在跳舞毯上运动，运动空间受到局限；(4)深度相机，如微软的Kinect，对干扰异常敏感，精度有限，识别的准确率较低，成本较高。
惯性测量单元(Inertial Measurement Unit，简称IMU)是测量物体三轴加速度以及姿态角(或角速率)的装置。一个IMU包含了三轴加速度计和三轴陀螺仪，加速度计检测物体在载体坐标系统中独立三轴的加速度信号，而陀螺仪则检测载体相对于导航坐标系的角速度信号。惯性测量单元在惯性导航领域应用广泛，测量得到人在运动过程中的加速度，然后对加速度二次积分，就能获得人的运动位移。
因此，对于用脚控制机械臂等执行机构的人机交互应用，急需一种空间限制较小，没有束缚的高精度控制方法，以人脚运动的位移作为控制信号，实现人机交互。
技术实现要素：
本实用新型的目的在于克服上述现有技术的不足，提供一种基于IMU的脚-机跟随控制系统，具体技术方案如下。
一种基于惯性测量单元的脚-机跟随控制系统，包括：(1)数据采集单元，用于采集工作脚的惯性数据(加速度和角速度值)；(2)数据传输单元，用于将惯性数据传输到控制单元以及将控制信号传输到执行器；(3)控制单元，将工作脚在传感器坐标系中的运动转换到大地坐标系中，同时把执行单元末端的运动也转换到大地坐标系中，根据转换后的工作脚惯性数据，将工作脚的运动状态映射到执行单元，发出控制信号；(4)执行单元，如机械臂，用于执行控制单元的命令；(5)交互启/停单元，用于控制工作脚与机械臂之间映射的连接与断开，调节工作脚映射到执行单元的比例系数。
IMU采集的数据为其中为三轴加速度数据，为三轴陀螺仪数据。而且该单元体积应较小，重量也应较轻，以便固定在人脚上。
数据传输单元应采用功耗较低，传输实时性好的有线或无线传输方式，如USB、串口、蓝牙等。
控制单元应采用运算能力强，并具有多种通信接口的控制单元，如PC、STM32等。执行单元应当具有多种运动自由度，并且支持多种通信方式，尤其是应当具有软件控制接口，且支持高级编程语言，例如可编程机械臂等机构。
交互启/停单元可选用一个带有按键和滑块的装置。辅助脚每次按下按键，都可以在工作脚与机械臂映射的连接与断开状态之间实现一次切换。辅助脚推动滑块左右运动，可以方便地调节映射比例系数k。
本实用新型提供的一种基于惯性测量单元的脚-机跟随控制系统，工作原理如下。
IMU采集的惯性数据是由真实的惯性数值sk(θ)和噪声vk组成的，即：
yk＝sk(θ)+vk
其中上标a和ω分别代表加速度和角速度，θ为未知的但是对IMU输出有影响的参数。在此假定噪声是独立同分布的，且都服从于均值为0，协方差矩阵为C的高斯分布，其中I3为3×3的单位矩阵，O3为3×3的零矩阵：
其中为实数，分别表示加速度计和陀螺仪的噪声方差，不同的IMU的并不相同，可以通过查IMU的技术手册计算求得。
IMU的安装方位是任意的。采用基于加速度、角速度的融合算法，解算得到IMU在大地坐标系中的姿态，将IMU在传感器坐标系中的运动转换到大地坐标系中。同时把机械臂的末端的运动也转换到大地坐标系中，方便设计工作脚运动和机械臂运动之间的映射。详细融合算法可以从参考文献(1)中获得。
数据采集单元采集到工作脚的惯性数据后，通过数据传输单元将工作脚的惯性数据传输到控制单元。控制单元通过上述现有技术的成熟算法，将IMU在传感器坐标系中的运动转换到大地坐标系中，同时把机械臂的末端的运动也转换到大地坐标系中。控制单元根据转换后的工作脚的惯性数据，将工作脚的运动状态映射到机械臂，发出控制信号。
机械臂跟随控制包含两种可选工作模式：轨迹跟随和位置跟随模式。轨迹跟随模式中，机械臂末端遵循工作脚的轨迹曲线，到达终点；位置跟随模式中，机械臂末端可以沿最近路径或其他规划路径，达到终点。
将工作脚的运动状态映射到机械臂的方法是设定一个比例系数k。在不同控制场景，工作脚的实际运动范围和期望的机械臂末端运动范围差异很大，为此设置比例系数k。在位置跟随模式中，工作脚的位移为p时，机械臂末端所实现的位移为k·p；在轨迹跟随模式中，工作脚的运动轨迹的曲率为c、长度为l时，机械臂末端所实现运动轨迹曲率为c，长度为k·l。k值越大，机械臂末端的运动范围越大，但运动精度越低；k值越小，机械臂末端的运动范围越小，但运动精度越高。
交互启/停单元由辅助脚控制，交互启/停单元可选用一个带有按键或滑块的装置。辅助脚每次按下按键，都可以在工作脚与机械臂映射的连接与断开状态之间实现一次切换。辅助脚推动滑块左右运动，可以方便地调节映射比例系数k。
相对于以往的人机交互应用，本实用新型提供的基于IMU的脚-机跟随控制系统，由于控制单元将工作脚在传感器坐标系中的运动转换到大地坐标系中，同时把机械臂的末端的运动也转换到大地坐标系中，IMU的安装方位是任意的；由于设置了映射的比例系数，所以对运动空间的大小没有限制；输出的位移已经是三轴的位移，因此能够实现对X、Y、Z三个方向的控制，而且可根据不同的应用场景，实现很多复杂的控制，控制的精度和实时性也较好。
附图说明
图1是本实用新型基于惯性测量单元的脚-机跟随控制系统的组成图；
图2是本实用新型实施例的基于惯性测量单元的脚-机跟随控制系统的连接图；
图3是本实用新型实施例的机械臂的三轴原始定义图；
图4是本实用新型实施例机械臂的轨迹跟随模式和位置跟随模式示意图。
具体实施方式
为了使本实用新型的目的、技术方案及优点更加清楚明白，以下结合附图对本实用新型进行进一步详细说明。应当理解，此处所描述的具体实施方式仅用以解释本实用新型，并不用于限定本实用新型。
图1是本实用新型基于惯性测量单元的脚-机跟随控制系统的组成图，图2是本实用新型实施例的基于IMU的脚-机跟随控制系统。IMU通过USB接口与PC相连，机械臂则通过串口与PC相连。但并不局限于串口的通信方式，本实用新型实施例采用的是有线连接的方式，若控制单元采用STM32，Arduino等进行无线通信，整个控制系统又能简化。
本实用新型的实施例选择Xsens公司生产的MTI-300系列IMU，该IMU可以输出较为精确的三轴加速度和角速度。IMU的采样频率设置为100Hz，即采样周期为0.01s。但是，并不局限于该IMU充当传感器，其他成本较低的IMU，在应用传感器融合算法，得出较为精确的三轴加速度和角速度后，也是可行的。
本实用新型的实施例选择越疆科技的DOBOT magician型机械臂充当控制对象，它可以沿X、Y、Z三个方向运动。机械臂的重复定位精度为0.02mm。该机械臂支持如蓝牙、串口、WiFi等多种通信方式，因而可以用多种不同的控制单元进行控制。本实用新型实施例使用PC的串口与机械臂进行通信，再通过PC上编写的C++程序，调用机械臂的控制接口，来实现对机械臂的控制。
本实用新型实施例的启停装置选择了一种带有按键和滑块的装置。按键只有踩下和松开两种状态，相当于一个开关信号，通过简单的单片机电路，可以将按键的状态通过蓝牙发送到PC上，从而控制机械臂的断开与连接；滑块的位置则相当于一个模拟信号，通过AD转换，也可以将滑块的相对位置通过蓝牙发送到PC。在本实用新型实施例中，按键踩下时连接机械臂，松开时则断开机械臂。
本实用新型实施例提供的基于惯性测量单元的脚-机跟随控制系统，具体包括：
1.按图2连接好系统，在人的左脚(工作脚)上固定IMU，右脚(辅助脚)控制按键和滑块，按键踩下时，在上位机程序中调用机械臂的控制接口连接机械臂，并重新初始化工作脚与机械臂的原点位置，其中机械臂的出厂原始坐标轴及原点定义如图3所示。按键松开时，则断开对机械臂的控制。
2.在机械臂连接的情况下，在上位机程序中调用IMU的数据接口，得到IMU测量的三轴加速度数据和三轴角速度数据；
3.将坐标系由传感器坐标系转换到大地坐标系，得到人脚在大地坐标系中的加速度值和位置信息。
4.将人脚的运动方向映射到机械臂中。在程序中调用机械臂的控制接口，就可以控制机械臂的运动。而且，传感器的X、Y、Z三轴刚好可以对应机械臂的三个方向，通过配置机械臂中控制运动方向的参数字，就可以让机械臂沿不同方向运动，达到跟随控制的目的。
5.跟随控制包含两种可选工作模式：轨迹跟随和位置跟随模式。以让机械臂从点A运动到点B为例，在本实施例中，轨迹跟随模式是让机械臂完全遵循工作脚的运动轨迹进行动作；位置跟随模式则是让机械臂直接沿直线从点A运动到点B。在轨迹跟随模式下，由于机械臂末端的运动轨迹是曲线，机械臂运动的距离相对较长，但是可以达到避障的目的；而在位置跟随模式下，机械臂末端的运动距离是最短的，因而控制的实时性较好。如图4所示。
6.在判断了机械臂的运动方向之后，还需要设定工作脚运动状态映射到机械臂的比例系数k。比例系数k可以通过辅助脚左右移动滑块进行更改，同时也可以在上位机界面中实时显示。在本实施例中，当滑块处于中间时，k的值为1；往左推动滑块时，k的值减小，机械臂末端的运动范围缩小，但运动精度变高；往右推动滑块时，k的值增大，机械臂末端的运动范围增大，但运动精度变低。
为了验证本实施例控制系统的性能，采用该控制系统用人脚控制机械臂从点(0,0,0)运动到点(200,200,200)(机械臂的实时坐标可以在上位机界面中监控)，并选用位置跟随模式，采用直线跟随：刚开始时用辅助脚向右移动滑块，将k设置为1.4，机械臂可以快速接近点(200,200,200)；在机械臂到达点(185.28,154.36,192.12)之后向左移动滑块，将k设置为0.2，以便机械臂能够较准确的到达指定点。最终机械臂能到达的点为(202.18,201.42,198.14)。若对精度有更高要求，可以继续减小k的值。.
以上所述为本实用新型的较佳实施例而已，但本实用新型不应该局限于该实施例和附图所公开的内容。所以凡是不脱离本实用新型所公开的精神下完成的等效或修改，都落入本实用新型保护的范围。
参考文献
[1]Skog I,Handel P,Nilsson J O,et al.Zero-velocity detection—An algorithm evaluation[J].IEEE Transactions on Biomedical Engineering,2010,57(11):2657-2666.