CN108748220A

CN108748220A - 一种基于惯性测量单元的脚-机跟随控制方法及控制系统

Info

Publication number: CN108748220A
Application number: CN201810525610.1A
Authority: CN
Inventors: 向馗; 曹雨阳; 景文昊; 庞牧野; 周申培; 唐必伟
Original assignee: Wuhan University of Technology WUT
Current assignee: Wuhan University of Technology WUT
Priority date: 2018-05-28
Filing date: 2018-05-28
Publication date: 2018-11-06
Anticipated expiration: 2038-05-28
Also published as: CN108748220B

Abstract

本发明公开了一种基于惯性测量单元(IMU)的脚‑机跟随控制方法及控制系统，属于模式识别和智能控制领域。该方法包括：(1)在人脚(工作脚)上固定一个IMU，采集工作脚的加速度，角速度数据，以解算足部的姿态，并进行坐标系转换；(2)进行零速检测，判断工作脚的运动状态——运动则对加速度进行积分并缓存积分结果，静止则对加速度进行校正并输出位移；(3)解算工作脚的运动状态并映射到机械臂中，达到跟随控制的目的，跟随控制包含两种可选工作模式：轨迹跟随和位置跟随模式；(4)在需要时，通过另外一只人脚(辅助脚)的动作，断开或重新连接工作脚与机械臂之间的映射，起到辅助调节作用。

Description

一种基于惯性测量单元的脚-机跟随控制方法及控制系统

技术领域

本发明属于模式识别和智能控制领域，涉及一种脚-机跟随控制方法及控制系统，尤其涉及一种基于惯性测量单元的脚-机跟随控制方法及控制系统。

背景技术

脚-机跟随控制，即用人脚去控制机械臂等执行器，使得人脚的运动能映射到机械臂中，机械臂能够跟随人脚的运动，属于人机交互的一种。随着微电子技术和传感器技术的发展以及虚拟现实技术的兴起，脚-机跟随控制的应用也越来越多。

已有的脚-机跟随系统包括如下几类：(1)操纵杆，操作困难，由于脚的灵活度不如手，即使对于健全的人，操纵杆的控制也比较困难；(2)踏板，功能有限，比如汽车的刹车，只有踩下和松开两种状态，往往还需要额外的机械结构；(3)游戏应用中的跳舞毯(脚踏键盘)，要求人脚在跳舞毯上运动，运动空间受到局限；(4)深度相机，如微软的Kinect，对干扰异常敏感，精度有限，识别的准确率较低，成本较高。

惯性测量单元(Inertial Measurement Unit，简称IMU)是测量物体三轴加速度以及姿态角(或角速率)的装置。一个IMU包含了三轴加速度计和三轴陀螺仪，加速度计检测物体在载体坐标系统中独立三轴的加速度信号，而陀螺仪则检测载体相对于导航坐标系的角速度信号。惯性测量单元在惯性导航领域应用广泛，测量得到人在运动过程中的加速度，然后对加速度二次积分，就能获得人的运动位移。由于积分的累积误差，这种位移估算方法的实际精度很差，无法正常使用。

因此，对于用脚控制机械臂等执行机构的人机交互应用，急需一种空间限制较小，没有束缚的高精度控制方法，以人脚运动的位移作为控制信号，实现人机交互。

发明内容

本发明的目的在于克服上述现有技术的不足，提供一种基于IMU的脚-机跟随控制方法及一种脚-机跟随控制系统。本发明采用的控制方法可以解算出脚的运动位移，因此能实现更为复杂的控制需求；该控制方法不是简单地对IMU输出的加速度进行二次积分，而是利用工作脚静止时速度为零的特点，不断对加速度进行校正，因此解算结果相对精确；采用无线传输方式，不需要额外的机械结构，对人体运动空间的限制也较小。本发明的目的通过以下技术方案实现：

1.在人脚(工作脚)上固定一个IMU，采集工作脚的加速度，角速度数据，以解算足部的姿态，并进行坐标系转换；

2.进行零速检测，判断工作脚的运动状态——运动则对加速度进行积分并缓存积分结果，静止则对加速度进行校正并输出位移；

3.解算工作脚的运动状态并映射到机械臂中，达到跟随控制的目的，跟随控制包含两种可选工作模式：轨迹跟随和位置跟随模式；

4.在需要时，通过另外一只人脚(辅助脚)的动作，断开或重新连接工作脚与机械臂之间的映射，起到辅助调节作用。

步骤1中IMU输出的惯性数据是由真实的惯性数值s_k(θ)和噪声v_k组成的，即：

y_k＝s_k(θ)+v_k

其中上标a和ω分别代表加速度和角速度，θ为未知的但是对IMU输出有影响的参数。在此假定噪声是独立同分布的，且都服从于均值为0，协方差矩阵为C的高斯分布，其中I₃为3×3的单位矩阵，O₃为3×3的零矩阵：

其中为实数，分别表示加速度计和陀螺仪的噪声方差，不同的IMU的并不相同，可以通过查IMU的技术手册计算求得。

进一步地，IMU的安装方位是任意的。采用基于加速度、角速度的融合算法，解算得到IMU在大地坐标系中的姿态，将IMU在传感器坐标系中的运动转换到大地坐标系中。同时把机械臂的末端的运动也转换到大地坐标系中，方便设计工作脚运动和机械臂运动之间的映射。详细融合算法可以从参考文献(3)中获得。步骤1中将传感器坐标系转换到大地坐标系，进行坐标系转换的目的是得到工作脚在大地坐标系中的位移。进行坐标系转换的前提是得到IMU的姿态。通过直接求解陀螺仪的四元数微分方程可以得到IMU的姿态四元数q，但陀螺仪的静态漂移很大，直接求解四元数微分方程得到的姿态不准确；加速度计的静态性能较好，但是动态性能不佳，仅用加速度也难以得到较准确的IMU的姿态。因此在本发明采用的控制方法中，采用了基于加速度、角速度的传感器融合算法，算法细节可以参考文献[3]。具体方法如下：

四元数可以表示IMU的姿态，假设真实的四元数为q，利用四元数表示加速度并减去IMU输出的三轴加速度，得到一个多元向量误差函数：

当此多元向量误差函数取得极小值时，认为得到的四元数最接近IMU真实的姿态。应用梯度下降等优化算法可以求得此函数的极小值假设通过求解陀螺仪的四元数微分方程得到的四元数为q_ω，将两种算法融合得到最终的四元数q_t：

选择合适的系数α，可以得到较为准确的IMU的姿态q_t。

进一步地，步骤2中零速检测[1]的具体方法如下：

当IMU运动时，其加速度和角速度反应的是在人体一段连续时间的运动状态，因为这一段时间IMU的角速度和加速度不仅取决于人运动的速度，还取决于人脚的实时姿态。因此，建立此运动状态的数学模型是很困难的。但当IMU静止时，IMU的加速度完全取决于重力加速度，而重力加速度的大小是已知的。再者，此时传感器的方位是恒定的，即IMU此时的角速度为零。若用假设表示传感器处在运动状态，用假设表示传感器处在静止状态，则对于这两种假设，用数学语言描述有：

满足s_k(θ)≠gu_n或者

都有s_k(θ)＝gu_n并且

这里，u_n∈Ω_u，Ω_u＝{u∈R³:║u║＝1}，即u_n是三维空间里的一个单位向量，而g为当地重力加速度的大小，为标量。

即在下，s_k(θ)是完全未知的，但而在下，只有加速度的方向未知。前面已经认为，加速度计和陀螺仪的误差服从高斯分布，因此可以求出两种假设下的极大似然估计。通过求解广义似然比校验，可以判断出人脚是处于静止还是运动状态。

进一步地，步骤2中输出位移的具体方法为零速更新[2]：

当人脚处于运动状态时，由于IMU存在漂移，在运动中直接对加速度二次积分得到的位移误差很大，因此暂时不输出位置数据。假设i＝0时刻检测到工作脚处于零速状态，经过一段时间运动，到i＝T时刻，重新检测到工作脚处于零速状态。则：

其中，Δt为采样周期。倘若通过步骤2判定出工作脚当前处于静止状态，理论上现在的vel的值为0，实际上由于IMU的加速度输出有大量噪声，vel的值并不为0，假设vel＝ε。为此对i＝0～T时刻的加速度值做校正：

得到校正后的加速度再对校正后的加速度二次积分得到位移Pos：

输出三轴位移pos，并将其映射到机械臂上，其中pos初始值为0。

进一步地，步骤3中，跟随控制包含两种可选工作模式：轨迹跟随和位置跟随模式。轨迹跟随模式中，机械臂末端遵循工作脚的轨迹曲线，到达终点；位置跟随模式中，机械臂末端可以沿最近路径或其他规划路径，达到终点。

进一步地，步骤3中，将工作脚的运动状态映射到机械臂的方法是设定一个比例系数k。在不同控制场景，工作脚的实际运动范围和期望的机械臂末端运动范围差异很大，为此设置比例系数k。在位置跟随模式中，工作脚的位移为p时，机械臂末端所实现的位移为k·p；在轨迹跟随模式中，工作脚的运动轨迹的曲率为c、长度为l时，机械臂末端所实现运动轨迹曲率为c，长度为k·l。k值越大，机械臂末端的运动范围越大，但运动精度越低；k值越小，机械臂末端的运动范围越小，但运动精度越高。

进一步地，步骤3中，为了抑制传感器漂移以及工作脚误动作引起机械臂末端抖动，还应在映射之前加入低通滤波算法，确保传递给机械臂末端的位移曲线是连续光滑的。滤波算法的设计，可以参考数字信号处理的教科书。

进一步地，步骤3中，脚-机跟随控制方法可采用PID控制(比例积分微分控制)算法，让机械臂末端能够准确迅速地达到映射位移的终点，以提高机械臂跟踪控制的快速性和稳定性。

进一步地，步骤4中，辅助脚控制一个带有按键或滑块的装置。辅助脚每次按下按键，都可以在工作脚与机械臂映射的连接与断开状态之间实现一次切换。辅助脚推动滑块左右运动，可以方便地调节映射比例系数k。

进一步地，步骤4中，每次重新实现工作脚与机械臂映射的连接时，都会重新初始化工作脚与机械臂的原点位置。即把当前工作脚和机械臂末端的位置，认定为它们在大地坐标系中的原点位置。位移或轨迹的映射，均以原点为参考点。

本发明的另一目的是提供一种基于惯性测量单元的脚-机跟随控制系统，具体技术方案如下：

(1)数据采集单元，用于采集惯性数据(加速度和角速度值)；(2)数据传输单元，用于将惯性数据传输到控制单元以及将控制信号传输到执行器；(3)控制单元，用于解算数据发出控制命令；(4)执行单元，如机械臂，用于执行控制单元的命令；(5)交互启/停单元，用于控制工作脚与机械臂之间映射的连接与断开，调节映射的比例系数。采用本发明提供的前述控制方法，实现控制单元对执行单元的控制。

IMU采集的数据为其中为三轴加速度数据，为三轴陀螺仪数据。而且该单元体积应较小，重量也应较轻，以便固定在人脚上。数据传输单元应采用功耗较低，传输实时性好的有线或无线传输方式，如USB、串口、蓝牙等。在控制算法中已经将传感器坐标系转换到大地坐标系，因此不需要向某个特定方向固定IMU。控制单元应采用运算能力强，并具有多种通信接口的控制单元，如PC、STM32等。执行单元应当具有多种运动自由度，并且支持多种通信方式，尤其是应当具有软件控制接口，且支持高级编程语言，例如可编程机械臂等机构。交互启/停单元可选用一个带有按键和滑块的装置。辅助脚每次按下按键，都可以在工作脚与机械臂映射的连接与断开状态之间实现一次切换。辅助脚推动滑块左右运动，可以方便地调节映射比例系数k。

相对于以往的人机交互应用，本发明提供的基于IMU的脚-机跟随控制系统，由于设置了映射的比例系数，所以对运动空间的大小没有限制；输出的位移已经是三轴的位移，因此能够实现对X、Y、Z三个方向的控制，而且可根据不同的应用场景，实现很多复杂的控制，控制的精度和实时性也较好。

附图说明

图1是本发明脚-机跟随控制方法的控制流程图；

图2是本发明基于惯性测量单元的脚-机跟随控制系统的组成图；

图3是本发明实施例的基于惯性测量单元的脚-机跟随控制系统的连接图；

图4是本发明实施例的机械臂的三轴原始定义图；

图5是本发明实施例机械臂的轨迹跟随模式和位置跟随模式示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

图3是本发明实施例的基于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.按图3连接好系统，在人的左脚(工作脚)上固定IMU，右脚(辅助脚)控制按键和滑块，按键踩下时，在上位机程序中调用机械臂的控制接口连接机械臂，并重新初始化工作脚与机械臂的原点位置，其中机械臂的出厂原始坐标轴及原点定义如图4所示。按键松开时，则断开对机械臂的控制。

2.在机械臂连接的情况下，在上位机程序中调用IMU的数据接口，得到IMU测量的三轴加速度数据acc和三轴角速度数据gyr；

3.通过编写的零速检测函数判断人脚处在运动状态还是静止状态，并将此状态缓存。

4.将得到的三轴加速度数据acc和三轴角速度数据gyr进行解算，得到IMU的姿态四元数quat。

5.得到四元数quat后，将坐标系由传感器坐标系转换到大地坐标系，得到人脚在大地坐标系中的加速度值acc_c。

6.当3中判断出人脚处于静止状态时，进行零速更新，对加速度进行校正，并输出位置信息pos。pos包含了三轴的位置信息pos.x，pos.y，pos.z。

7.将人脚的运动方向pos映射到机械臂中。在程序中调用机械臂的控制接口，就可以控制机械臂的运动。而且，传感器的X、Y、Z三轴刚好可以对应机械臂的三个方向，通过配置机械臂中控制运动方向的参数字，就可以让机械臂沿不同方向运动，达到跟随控制的目的。

8.跟随控制包含两种可选工作模式：轨迹跟随和位置跟随模式。以让机械臂从点A运动到点B为例，在本实施例中，轨迹跟随模式是让机械臂完全遵循工作脚的运动轨迹进行动作；位置跟随模式则是让机械臂直接沿直线从点A运动到点B。在轨迹跟随模式下，由于机械臂末端的运动轨迹是曲线，机械臂运动的距离相对较长，但是可以达到避障的目的；而在位置跟随模式下，机械臂末端的运动距离是最短的，因而控制的实时性较好。如图5所示。

9.为了抑制传感器漂移以及工作脚误动作引起机械臂末端抖动，在程序中加入了低通滤波算法，确保传递给机械臂末端的位移曲线是连续光滑的。

10.为了能够使人脚能快速、稳定地控制机械臂到达映射位移的终点，在程序中加入了PID控制策略。PID参数已经通过多次试验人工整定。

11.在判断了机械臂的运动方向之后，还需要设定工作脚运动状态映射到机械臂的比例系数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的值。.

本实施例应用了本发明提出的基于惯性测量单元的脚-机跟随控制方法，使得人脚的运动位移，能完全映射到机械臂上，并且在控制单元中加入了PID控制策略和低通滤波算法，使得控制准确、快速，而且能在一定程度上抑制噪声干扰，运动的空间也不局限于二维平面。本领域的技术人员可以理解上述实施例中的全部或部分流程，通过修改程序逻辑和映射关系来实现不同的应用。

以上所述为本发明的较佳实施例而已，但本发明不应该局限于该实施例和附图所公开的内容。所以凡是不脱离本发明所公开的精神下完成的等效或修改，都落入本发明保护的范围。参考文献

[1]Skog I,Handel P,Nilsson J O,et al.Zero-velocity detection—Analgorithm evaluation[J].IEEE Transactions on Biomedical Engineering,2010,57(11):2657-2666.

[2]Yun X,Calusdian J,Bachmann E R,et al.Estimation of human footmotion during normal walking using inertial and magnetic sensor measurements[J].IEEE Transactions on Instrumentation and Measurement,2012,61(7):2059-2072.

[3]Madgwick S O H,Harrison A J L,Vaidyanathan R.Estimation of IMU andMARG orientation using a gradient descent algorithm[C]//RehabilitationRobotics(ICORR),2011IEEE International Conference on.IEEE,2011:1-7.

Claims

1.一种基于惯性测量单元的脚-机跟随控制方法，其特征在于，包括以下步骤：

(1)在工作脚上固定一个IMU，采集工作脚的加速度，角速度数据，解算足部的姿态，并进行坐标系转换；

(2)进行零速检测，判断工作脚的运动状态：运动则对加速度进行积分并缓存积分结果，静止则对加速度进行校正并输出位移；

(3)解算工作脚的运动状态并映射到机械臂中，实现对机械臂的跟随控制。

2.根据权利要求1所述的控制方法，其特征在于，还包括步骤(4)：

(4)通过辅助脚的动作，断开或重新连接工作脚与机械臂之间的映射，调节映射比例系数。

3.根据权利要求1所述的控制方法，其特征在于，步骤(1)中采用基于加速度、角速度的融合算法，解算得到IMU在大地坐标系中的姿态，将IMU在传感器坐标系中的运动转换到大地坐标系中；同时把机械臂末端的运动也转换到大地坐标系中。

4.根据权利要求1所述的控制方法，其特征在于，步骤(2)中零速检测的方法为：用假设表示传感器处在运动状态，用假设表示传感器处在静止状态，

满足s_k(θ)≠gu_n或者

都有s_k(θ)＝gu_n并且

求出两种假设下的极大似然估计后校验，判断出人脚是处于静止还是运动状态。

5.根据权利要求1所述的控制方法，其特征在于，步骤(2)中计算输出位移的方法是零速更新：

假设i＝0时刻检测到工作脚处于零速状态，经过一段时间运动，到i＝T时刻，重新检测到工作脚处于零速状态，则

其中，Δt为采样周期；

假设vel＝ε，对i＝0～T时刻的加速度值做校正，得到校正后的加速度

为此，在i＝0～T时刻，工作脚的运动位移为：

6.根据权利要求1所述的控制方法，其特征在于，步骤(3)中，跟随控制包含两种可选工作模式：轨迹跟随模式或位置跟随模式；轨迹跟随模式中，机械臂末端遵循工作脚的轨迹曲线，到达终点；位置跟随模式中，机械臂末端沿最近路径或其他规划路径，到达终点。

7.根据权利要求6所述的控制方法，其特征在于，步骤(3)中将工作脚的运动状态映射到机械臂的方法是设定一个比例系数k：在轨迹跟随模式中，工作脚的运动轨迹的曲率为c、长度为l时，机械臂末端所实现运动轨迹曲率为c，长度为k·l；在位置跟随模式中，工作脚的位移为p时，机械臂末端所实现的位移为k·p。

8.根据权利要求1所述的控制方法，其特征在于，步骤(3)中，在映射之前加入低通滤波算法。

9.根据权利要求1所述的控制方法，其特征在于，步骤(3)中，脚-机跟随控制方法采用PID控制算法。

10.根据权利要求2所述的控制方法，其特征在于，步骤(4)中，辅助脚控制一个带有按键和滑块的装置：辅助脚每次按下按键，在工作脚与机械臂映射的连接与断开状态之间实现一次切换；辅助脚推动滑块左右运动，调节映射比例系数。

11.根据权利要求10所述的控制方法，其特征在于，步骤(4)中，每次重新实现工作脚与机械臂映射的连接时，都重新初始化工作脚与机械臂的原点位置。

12.一种基于惯性测量单元的脚-机跟随控制系统，包括：(1)数据采集单元，用于采集惯性数据；(2)数据传输单元，用于将惯性数据传输到控制单元以及将控制信号传输到执行器；(3)控制单元，用于解算数据发出控制命令；(4)执行单元，用于执行控制单元的命令；(5)交互启/停单元，用于控制工作脚与机械臂之间映射的连接与断开，调节映射的比例系数；其特征在于，采用权利要求1-11任一权利要求所述的控制方法，实现控制单元对执行单元的控制。

13.根据权利要求12所述的脚-机跟随控制系统，其特征在于，所述惯性测量单元采集的数据为其中为三轴加速度数据，为三轴陀螺仪数据；数据传输单元采用的传输方式为USB、串口或蓝牙；控制单元为PC或STM32；执行单元为可编程机械臂；交互启/停单元为带有按键和滑块的装置。