CN109108970A - 一种基于骨骼节点信息的交互式机械臂控制方法 - Google Patents

Abstract

本发明提出了一种基于骨骼节点信息的交互式机械臂控制方法，采用空间向量法计算出操控人员肘关节、腕关节以及肩关节的转动角度，使用限幅加权滑动平均滤波算法对各转动角度进行滤波处理；并将经滤波处理的转动角度作为控制指令的传入参数，根据所述含参控制指令控制机械臂。有益效果：该方法能够实现体感交互示教的功能且具有精度高、延时小等优点。

Description

一种基于骨骼节点信息的交互式机械臂控制方法

技术领域

本发明属于计算机视觉技术领域，尤其涉及一种基于骨骼节点信息的交互式 机械臂控制方法。

背景技术

为了给老年人和残疾人提供性能优越的操作工具，提高他们的行动自由度， 许多国家对移动机器人进行了研究。在这些移动式机器人当中，装有机械臂的机 器人能代替人执行一些高危、繁重的工作。然而现存的机械臂主要是通过专业人 员输入操作指令，控制机械臂，这极大限制了其他非专业人员对机械臂的操作和 使用。因此建立一个体感交互式机械臂控制系统便于其他人员通过自身的肢体动 作控制机械臂，非常有现实意义。

近年来国内外一些研究人员对体感交互方面展开了研究。Igorevich等人[文 献1](Igorevich R R,Ismoilovich E P,Min D.Behavioral Synchronization of Humanand Humanoid Robot[C].International Conference on Ubiquitous Robots andAmbient Intelligence,2012:655-660.)设计了基于Kinect的仿人机器人的运动控制系统， 利用Kinect传感器进行人体骨骼检测与跟踪，控制机器人运动；周小模等人[文 献2](周小模,刘威.基于Kinect与Dobot机械臂的人机交互系统[J].信息技术, 2017(12):102-104.)通过Kinect捕获手部的坐标移动以及手势变化，利用坐标转 化等方法，将其转化为指令传送给机械臂，实现了机械手抓取物体等功能；胡星 晨等人[文献3](胡星晨,李波,赵祎临.基于Kinect的体感交互机器人[J].电子测 量技术,2017(8):139-143.)利用Kinect的骨骼追踪功能获取人体主要关节点的坐 标，然后转换为机械臂可用数据并计算出关节角，最后通过无线传输方式将关节 角发送给机器人控制器，建立了机械臂控制系统，从而控制机器人做出和人相同 的动作；邵阳等人[文献4](邵阳,战荫.余弦DTW在上肢康复训练中的应用[J].计 算机工程与设计,2018,39(01):249-254.)在Kinect平台下建立一个康复训练系统， 通过采集关节点的三维坐标信息，将每对相邻的关节点用向量形式表示，以向量 变化的时间序列代替运动轨迹，实现了6种上肢训练动作；邵泽燕等人[文献5] (邵泽燕,邵泽健,陈成军.基于Kinect的机械臂增强现实示教研究[J].计算技术 与自动化,2017,36(02):68-72.)通过Kinect传感器采集示教者手部3D位置数据， 利用机器人逆运动学模型驱动虚拟机械臂模型跟踪人手运动，将虚拟机械臂模型 叠加到真实工作场景中，实现对机械臂的示教功能；周彤彤等人[文献6](周彤 彤,刘更谦,李浩源.基于Kinect的双臂机器人动作模拟与轨迹规划[J].科学技术 与工程,2017,17(07):58-62.)提出了一种基于Kinect的双臂机器人的运动仿真和 轨迹规划方法，通过建立人体的关节点和机器人节点之间的映射关系，通过无线 传输模式将传感器收集到的信息传送给机器人，可以实现机器人的运动仿真；顾 客等人[文献12](顾客,何益明,金丽燕.基于Kinect骨骼追踪技术的智能教学 控制系统的设计与实现[J].计算机应用与软件,2013,30(6):276-280.)通过Kinect 实时准确地捕获人体的骨骼运动轨迹和骨骼点信息，把三维骨骼数据的坐标系转化为二维数据坐标系，研发出了基于体感控制的智能教学系统。这些方法主要是 通过传感器设备采集人体的肢体动作，然后将肢体动作信息转化成控制指令，最 后传输给机械臂控制器，实现了在不同应用场景下的体感交互功能，由于没有考 虑在实际采集数据时角度过大或过小会容易产生采集误差，导致体感交互时的精 度较低；此外，在数据传输到机械臂控制器的过程中，存在较大的延时现象。

发明内容

本发明的目的在于克服上述现有技术之不足，提出了一种基于骨骼节点信息 的交互式机械臂控制方法，具体由以下技术方案实现：

所述基于骨骼节点信息的交互式机械臂控制方法，其特征在于采用空间向 量法计算出操控人员肘关节、腕关节以及肩关节的转动角度，使用限幅加权滑动 平均滤波算法对各转动角度进行滤波处理；并将经滤波处理的转动角度转化为对 应的控制指令，根据所述控制指令控制机械臂。

所述基于骨骼节点信息的交互式机械臂控制方法的进一步设计在于，所述 方法具体包括如下步骤：

步骤1)骨骼信息采集：通过Kinect红外发射器向操控人员投射近红外线，红外 相机接收反射回来的红外光线，计算光线发射和反射时间差，采用时间片技术测 量人体距离，生成深度图像流，通过所述深度图像流提取各个关节点，获得骨骼 坐标信息；

步骤2)坐标中心化和归一化：将人体颈部骨骼点和脊椎骨骼点之间的长度作为标准长度，对骨骼框架中的节点进行归一化处理；

步骤3)坐标映射与空间向量：根据对机械臂舵机控制的需求，以O为原点建立 三维坐标系，选择右腕关节点W(w_x,w_y,w_z)、右肘关节点E(e_x,e_y,e_z)、右肩关 节点S(s_x,s_y,s_z)，将三个关节点的坐标信息转换为空间向量SO、SE、ES、EW， 并表征肘关节、腕关节、肩关节的转动角度分别为α、β和θ；

步骤4)计算肩关节的转动角度θ、腕关节的转动角度β以及肘关节的转动角度 α；

步骤5)对肩关节、腕关节、肘关节的转动角度进行滤波：采用限幅加权滑动平 均滤波算法，对每个队列中的关节转动角度赋予不同的权值，再求加权平均值， 得到最终估值；

步骤6)交互式控制：将计算出的人体肩、肘关节转动角度值拟合成控制指令的 传入参数，将控制指令的传入参数与各个舵机的脉冲宽度调制值形成映射，控制 机械臂响应；记录机械臂随人体手臂运动的数据，完成动作复现。

所述基于骨骼节点信息的交互式机械臂控制方法的进一步设计在于，所述步 骤3)中以O为原点建立三维坐标系，选择右腕关节点W(w_x,w_y,w_z)、右肘关节 点E(e_x,e_y,e_z)、右肩关节点S(s_x,s_y,s_z)，将三个关节点的坐标信息转换为空间 向量SO、SE、ES、EW。

所述基于骨骼节点信息的交互式机械臂控制方法的进一步设计在于，所述步 骤4)中计算肩关节的转动角度θ时，将操控人员的右肩关节点S和右肘关节点 E连线并延长至x轴，且与x轴负向夹角为90°-θ，根据式(1)计算肩关节的 转动角度。

θ＝90°-arctan((s_y-e_y)/(e_x-s_x)) (1)

所述基于骨骼节点信息的交互式机械臂控制方法的进一步设计在于，所述步 骤4)中计算转动转动角度β时，β为向量SE和向量SO的夹角， SE＝(e_x-s_x,e_y-s_y,e_z-s_z)，SO＝(s_x,s_y,s_z)，根据式(2)计算角度β。

所述基于骨骼节点信息的交互式机械臂控制方法的进一步设计在于，所述步 骤4)中计算转动角度α时，向量ES＝(s_x-e_x,s_y-e_y,s_z-e_z)，向量 EW＝(w_x-e_x,w_y-e_y,w_z-e_z)，根据式(3)计算肘关节转动角度α。

所述基于骨骼节点信息的交互式机械臂控制方法的进一步设计在于，所述步 骤5)中从一个有n项的时间序列中计算多个连续m项序列平均值，将N个采 样到的数组成队列，求出平均值，之后每次新加入一个数到队列，队列中原来的 元素向前移一个位置，第一个元素被舍弃，将最新的数放到队尾，再求新的队列 中所有元素平均值，达到限幅滤波效果；并通过对每个队列中的关节转动角度赋 予不同的权值，再求加权平均值，得到最终估值，根据式(4)构建数学模型：

Val＝W₁X₁+W₂X₂+...+W_NX_N (4)

其中，N为采样到的数组队列，W_i为权重，i∈[1,N]，且W₁≥W₂≥...≥W_N；

X₁为当前检测值，X₂为前一次检测值，X_N为前N次检测值，Val为最终估值。

所述基于骨骼节点信息的交互式机械臂控制方法的进一步设计在于，所述步 骤6)中将计算出的人体肩、肘关节转动角度值拟合成控制指令的传入参数；采 用中断查询接收的方式从串口接收数据并控制六自由度机械臂；各个角度值对应 为各个舵机的PWM值，当张开角度小于30°时，角度值和PWM的给定值构成 三次曲线；当张开角度大于30°时，将角度值和PWM的给定值构成一次函数， 并且为保证连续，规定在两个临界点的数值相等。

所述基于骨骼节点信息的交互式机械臂控制方法的进一步设计在于，所述控 制命令的数据帧格式为：1位值为0的起始位；8位数据位，每两位代表一路舵 机角度；1位值为1的停止位。

所述基于骨骼节点信息的交互式机械臂控制方法的进一步设计在于，所述步 骤6)中所述控制指令通过蓝牙串口发送给单片机，所述单片机根据控制指令控 制机械臂响应。

有益效果：

本发明的基于骨骼节点信息的交互式机械臂控制方法，是一种延时小、精确 度高的示教技术。该方法中采用空间向量的形式计算出各关节的转动角度，使用 限幅加权滑动平均滤波算法对关节转动角度数据进行滤波处理，实现了从实际的 三维空间到坐标系的映射关系，减小了数据采集和计算过程中存在的误差；将关 节转动角度数据转化为对应控制指令的传入参数，通过蓝牙串口将控制指令发送 给单片机，降低了数据传输的延时，且精度高；设计界面，提高可视化效果。

附图说明

图1本发明关节点的坐标信息空间映射图。

图2本发明机械臂控制命令的数据帧格式图。

图3本发明PWM的值与角度的关系图。

具体实施方式

结合具体实施例与附图对本发明的技术方案进一步说明。

本发明提供的基于骨骼节点信息的交互式机械臂控制方法，采用空间向量法 计算出各关节的转动角度，使用限幅加权滑动平均滤波算法对关节转动角度进行 滤波处理；将关节转动角度转化为对应控制指令的传入参数，通过蓝牙串口将控 制指令发送给单片机控制机械臂；其具体实现包括以下步骤：

步骤1)：Kinect红外发射器投射近红外线到人体并发生反射，红外相机接收 反射回来的红外光线，计算光线发射和反射时间差，采用时间片技术测量人体距 离，生成深度图像流，然后由这些深度数据提取各个关节点，获得骨骼坐标信息；

步骤2)：不同的操作者在采集人体骨骼节点信息过程中与Kinect相对位置 不同，所做的动作轨迹也会有很大的不同，如果直接计算会造成很大的误差，对 后续的识别会造成影响，再加上操作者的体形大小不一，骨骼节点对应的位置也 会有偏差，同时也会对采集的深度三维数据产生影响，所以需要对三维骨骼数据 进行中心化和归一化处理。由于在动态识别过程中人体颈部骨骼点和脊椎骨骼点 之间的相对位移最小，故选取这两点之间的长度作为标准长度，对骨骼框架中的 节点进行归一化处理。

步骤3)：以O为原点建立三维坐标系，选择右腕关节点W(w_x,w_y,w_z)、右 肘关节点E(e_x,e_y,e_z)、右肩关节点S(s_x,s_y,s_z)，将三个关节点的坐标信息转换 为空间向量SO、SE、ES、EW，并表征肘关节、腕关节、肩关节的转动角度 分别为α、β和θ。

步骤4)：肩关节点S和右肘关节点E连接并延长，相交于x轴且设与x轴负 向夹角为90°-θ，求得肩关节的转动角度为：

θ＝90°-arctan((s_y-e_y)/(e_x-s_x)) (7)

步骤5)：计算腕关节的转动角度β：角度β为向量SE和向量SO的夹角， SE＝(e_x-s_x,e_y-s_y,e_z-s_z)，SO＝(s_x,s_y,s_z)，故β的计算如下：

步骤6)：计算肘关节的转动角度α：向量ES＝(s_x-e_x,s_y-e_y,s_z-e_z)， 向量EW＝(w_x-e_x,w_y-e_y,w_z-e_z)，则肘关节转动角度α如下：

步骤7)：从一个有n项的时间序列中计算多个连续m项序列平均值，把N 个采样到的数组成队列，求出平均值，之后每次新加入一个数到队列，队列中原 来的元素向前移一个位置，第一个元素被舍弃，将最新的数放到队尾，再求新的 队列中所有元素平均值，达到限幅滤波效果。对于本次采样到的关节转动角度影 响最大的是上一次采集到的关节转动角度，之前的关节转动角度数据对于本次的 影响逐次递减，由此引出加权滑动平均滤波。通过对每个队列中的关节转动角度 赋予不同的权值，然后求加权平均值，得到最终估值，数学模型如下：

Val＝W₁X₁+W₂X₂+...+W_NX_N (4)

其中，W_i(i∈[1,N])为权重，且W₁≥W₂≥...≥W_N；X₁为当前检测值，X₂为 前一次检测值，X_N为前N次检测值，Val为最终估值；在此控制系统中，经过多 次实践调整，采用了当前检测值和前面四个值共五个数据，权值分别为0.4、0.3、 0.2、0.05、0.05。

步骤8)：在机械臂控制部分，将前一部分中计算出的人体肩、肘关节转动 角度值拟合成控制指令的传入参数，整个控制命令的数据帧格式为：1位起始位 值为0；8位数据位，每两位代表一路舵机角度；1位停止位值为1；然后通过蓝 牙串口将控制指令发送给单片机，串口通信波特率设为115200bps，单片机则采 用中断查询接收的方式从串口接收数据。机械臂的伺服舵机接收控制信号输出角 度，它由直流伺服电机控制芯片直接接收脉宽可调的方波信号PWM，方波脉冲 信号的周期为10ms，频率100Hz，当方波脉宽改变时，舵机转轴的角度改变， 角度变化与脉宽的变化成正比，因此，利用占空比的变化就可以改变舵机的转角 位置。采用S3010舵机，供电电压为6V，输出扭矩为7.8Kgfcm。机械臂的张开 角度以90°为参考点，当张开角度值小于30°时，将角度值和PWM的给定值构造 成三次曲线；当角度大于30°时，将角度值和PWM的给定值构造成一次函数， 并且为保证连续，规定在两个临界点的数值相等。

步骤9)：界面设计，编写上位机显示Kinect采集到的图像，并添加了“示 教开始”、“示教结束”、“复现”、“复位”等功能键。

本申请的发明人对本发明方法的效果进行了实验验证，根据操作者肘关节、 肩关节运动的角度，计算理论检测角度，利用角位移传感器测出舵机实际转动角 度；分别计算3个误差值，其中误差1是关节运动角度和检测角度之间差异；误 差2是指检测角度和舵机转动角度之间差异；误差3是关节的运动角度和舵机转 动角度之间差异。经过多次实验，抽取几组数据的均值用来分析该系统的误差， 如表1～3所示。

表1肘关节角度的测试误差

表2肩关节上下角度的测试误差

表3肩关节前后角度的测试误差

由实验数据可知，Kinect采集演示者的骨骼点信息时存在噪声，即使经过限 幅加权滤波处理后，舵机的转动角度和检测角度之间仍然存在一定的误差，且在 起始角度和终止角度误差最为明显；总体而言该机械臂转动的误差小，系统的精 度高达95.65％，实现了体感交互示教的功能且具有精度高等优点。实验设备配 置为：操作系统和版本Windows10/Visual Studio 2013，2.5Ghz主频四核Core i5CPU，支撑环境.net framework 4.0及以上，Microsoft Xbox驱动，USB 3.0数据 接口，Microsoft Kinect 2.0，64位(x64)处理器，专用的USB 3.0总线(英特 尔和瑞萨控制器)，2GB RAM，NVIDIA Geforce 610m显卡，Windows Server 2012 及以上网络环境。

本发明方法基于Kinect传感器采集骨骼节点信息，采用空间向量的形式计算 出各关节的转动角度，使用限幅加权滑动平均滤波算法对关节转动角度数据进行 滤波处理，实现了从实际的三维空间到坐标系的映射关系，减小了数据采集和计 算过程中存在的误差；将关节转动角度数据转化为对应控制指令的传入参数，通 过蓝牙串口将控制指令发送给单片机，降低了数据传输的延时，且精度高；设计 界面，提高可视化效果。在体感交互式示教方面具有延时小、精度高等优点。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限 于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到 的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应 该以权利要求的保护范围为准。

Claims (10)

1.一种基于骨骼节点信息的交互式机械臂控制方法，其特征在于采用空间向量法计算出操控人员肘关节、腕关节以及肩关节的转动角度，使用限幅加权滑动平均滤波算法对各转动角度进行滤波处理；并将经滤波处理的转动角度转化为对应控制指令的传入参数，根据所述控制指令控制机械臂。

2.根据权利要求1所述的基于骨骼节点信息的交互式机械臂控制方法，其特征在于所述方法具体包括如下步骤：

步骤1)骨骼信息采集：通过Kinect红外发射器向操控人员投射近红外线，红外相机接收反射回来的红外光线，计算光线发射和反射时间差，采用时间片技术测量人体距离，生成深度图像流，通过所述深度图像流提取各个关节点，获得骨骼坐标信息；

步骤2)坐标中心化和归一化：将人体颈部骨骼点和脊椎骨骼点之间的长度作为标准长度，对骨架中的所有节点进行归一化处理；

步骤3)坐标映射与空间向量：根据对机械臂舵机控制的需求，以O为原点建立三维坐标系，选择右腕关节点W(w_x,w_y,w_z)、右肘关节点E(e_x,e_y,e_z)、右肩关节点S(s_x,s_y,s_z)，将三个关节点的坐标信息转换为空间向量SO、SE、ES、EW，并表征肘关节、腕关节、肩关节的转动角度分别为α、β和θ；

步骤4)计算肩关节的转动角度θ、腕关节的转动角度β以及肘关节的转动角度α；

步骤5)对肩关节、腕关节、肘关节的转动角度进行滤波：采用限幅加权滑动平均滤波算法，对每个队列中的关节转动角度赋予不同的权值，再求加权平均值，得到最终估值；

步骤6)交互式控制：将计算出的人体肩、肘关节转动角度值拟合成控制指令的传入参数，将控制指令的传入参数与各个舵机的脉冲宽度调制值形成映射，控制机械臂响应；记录机械臂随人体手臂运动的数据，完成动作复现。

3.根据权利要求2所述的基于骨骼节点信息的交互式机械臂控制方法，其特征在于所述步骤3)中以O为原点建立三维坐标系，选择右腕关节点W(w_x,w_y,w_z)、右肘关节点E(e_x,e_y,e_z)、右肩关节点S(s_x,s_y,s_z)，将三个关节点的坐标信息转换为空间向量SO、SE、ES、EW。

4.根据权利要求2所述的基于骨骼节点信息的交互式机械臂控制方法，其特征在于所述步骤4)中计算肩关节的转动角度θ时，将操控人员的右肩关节点S和右肘关节点E连线并延长至x轴，与x轴负向的夹角为90°-θ，根据式(1)计算肩关节的转动角度

θ＝90°-arctan((s_y-e_y)/(e_x-s_x)) (1)。

5.根据权利要求2所述的基于骨骼节点信息的交互式机械臂控制方法，其特征在于所述步骤4)中计算转动角度β时，β为向量SE和向量SO的夹角，SE＝(e_x-s_x,e_y-s_y,e_z-s_z)，SO＝(s_x,s_y,s_z)，根据式(2)计算角度β

6.根据权利要求2所述的基于骨骼节点信息的交互式机械臂控制方法，其特征在于所述步骤4)中计算转动角度α时，向量ES＝(s_x-e_x,s_y-e_y,s_z-e_z)，向量EW＝(w_x-e_x,w_y-e_y,w_z-e_z)，根据式(3)计算肘关节转动角度α

7.根据权利要求2所述的基于骨骼节点信息的交互式机械臂控制方法，其特征在于所述步骤5)中从一个有n项的时间序列中计算多个连续m项序列平均值，将N个采样到的数组成队列，求出平均值，之后每次新加入一个数到队列，队列中原来的元素向前移一个位置，第一个元素被舍弃，将最新的数放到队尾，再求新的队列中所有元素平均值，达到限幅滤波效果；并通过对每个队列中的关节转动角度赋予不同的权值，再求加权平均值，得到最终估值，根据式(4)构建数学模型：

Val＝W₁X₁+W₂X₂+...+W_NX_N (4)

其中，N为采样到的数组队列，W_i为权重，i∈[1,N]，且W₁≥W₂≥...≥W_N；X₁为当前检测值，X₂为前一次检测值，X_N为前N次检测值，Val为最终估值。

8.根据权利要求2所述的基于骨骼节点信息的交互式机械臂控制方法，其特征在于所述步骤6)中将计算出的人体肩、肘关节转动角度值拟合成控制指令的传入参数；采用中断查询接收的方式从串口接收数据并控制六自由度机械臂；各个角度值对应各个舵机的PWM值，当张开角度小于30°时，角度值和PWM的给定值构成三次曲线；当张开角度大于30°时，将角度值和PWM的给定值构成一次函数，并且为保证连续，规定在两个临界点的数值相等。

9.根据权利要求8所述的基于骨骼节点信息的交互式机械臂控制方法，其特征在于所述控制命令的数据帧格式为：1位值为0的起始位；8位数据位，每两位代表一路舵机角度；1位值为1的停止位。

10.根据权利要求2所述的基于骨骼节点信息的交互式机械臂控制方法，其特征在于所述步骤6)中所述控制指令通过蓝牙串口发送给单片机，所述单片机根据控制指令控制机械臂响应。