轻型仿人机械手臂集成研发平台ArmPlant的设计与实现

杨 华，闫雨奇，苏势林，董竹庆

(沈阳航空航天大学 计算机学院，沈阳 110136)

人类手臂在运动和劳动过程中表现出的灵活性、柔韧性、适应性无与伦比。从1968年美国通用电气试制 Rig开始至今，仿人机器人的研制已近50年历史。美国波斯顿动力公司的Atlas和日本本田公司的ASIMO代表世界先进水平，但其机械手臂在灵活性、柔韧性、负载力等方面与人类手臂相比仍有一定差距[1-2]。仿人机械手臂的研制过程极具挑战性，通常包括形制建模、计算仿真、样机试制、驱动控制、评价改进等环节，横跨机械、电子、计算机、控制、仿生学等多专业领域。一直以来缺少系统、专门的集成开发平台,各研究机构在研制过程中经常要混搭各种工具，或者创建自己的专用平台[3-5],这实际上阻碍了仿人机械手臂乃至仿人机器人的研发过程。

根据实现方式不同，机械手臂建模和仿真方法可分为基于动力学计算和Matlab的联合仿真、自建仿真平台。目前使用较多的是用三维实体模型和Matlab的联合仿真[6-8]。要验证手臂运动情况，需要编写接口程序，再将手臂模型导入Matlab，编写算法控制。对机械系统的运动学和动力学进行比较准确的仿真分析，但是缺少灵活性和针对性。相比之下，自建仿真实验平台有较强的灵活性和针对性，系统将建模、仿真等功能组合在一起，无需过多关注底层实现，提高研发效率。

本文基于ODE(Open Dynamic Engine)物理引擎[16]，采用Visual C++、Matlab混合编程开发了一个轻型仿人机械手臂集成研发平台ArmPlant。该平台可大量节约开发时间，减少底层代码编写，支持直接设置参数，操作简洁。实践过程中，基于该平台设计并实现了一个8-DoF(自由度)仿人轻型机械手臂，验证了该平台的有效性和实用性。

1 ArmPlant设计

ArmPlant支持仿人机械手臂的研发全过程，采用模块化设计，为提高响应和控制速度，采用Windows多线程设计。

(1)界面控制线程：负责人机交互，是仿真平台的主线程，完成对手臂样机的设计和实时控制，实时接收设计者命令并反馈运行状态，交互界面如图1所示；

(2)运动计算和控制线程：根据运动规划实时计算并监测各关节的旋转角度、速度等，从而控制整个手臂的轨迹；

(3)硬件驱动线程：负责与下位机(Arduino开发板)通信，控制物理样机和仿真模型同步完成相同动作。

1.1 ArmPlant架构

ArmPlant架构如图2所示，主要功能可分为两大部分：手臂的设计与仿真、样机驱动控制。

(1)设计和仿真部分：支持5个功能：1)手臂的仿真搭建：支持手臂的连杆外形、尺寸、质量分布、电机力矩范围等设置。2)运动学计算：支持运动学及逆运动学运算，通过手臂末端的位置来控制手臂的运动，或者通过各个关节角度计算出手臂末端的位置坐标。3)力矩的测试：支持各个关节在运动过程中的力矩测试。4)运动轨迹的编辑和加载：支持运动轨迹规划及编辑、纪录运动过程中每个时刻各个关键点的位置并完成保存，同样支持现有轨迹的加载。5)支持基于Kinect体感设备的人体动作数据的转化和驱动：支持对Kinect体感设备采集的BVH数据转化，提出一种基于逆运动学的关节位置点转化成关节角度的去冗余算法，从而驱动手臂的运动。

(2)物理样机的驱动控制：该模块主要支持物理样机的驱动功能。通过串口通信实现上位机与下位机的通信，从而对手臂样机的位置、关节角度、速度、加速度进行控制。实现对样机的运动轨迹设定、运动轨迹的加载、末端夹具等进行控制。

图1 ArmPlant交互界面图

图2 Armplant架构框图

1.2 手臂运动模型

以左臂为例，人体手臂可视作由2个连杆(大臂、前臂)和3个关节(肩关节、肘关节、腕关节)链接组成，共计7个自由度(不包含手部)[9]，其中手部简化为1-DoF末端夹具，如图3(a)所示，共包括8个自由度(肩部3个、肘部1个，腕部3个，手部1个)，各关节的活动范围根据手臂的范围设置，如表1所示。仿人机械手臂的形制完全按照成人手臂尺寸、关节自由度及运动范围设计，建立模型如图3(b)所示，包括8个自由度，其中肩部2个、上臂1个、肘部1个，前臂1个、腕部2个，末端夹持1个。采用ODE编程构建与人类物理尺寸和运动特征相符合的虚拟仿人机械人手臂。ODE是工业级刚体动力学库[10]，其力学特性、质量分布、碰撞检测等方面与真实物理世界相符合。

图3 人体手臂的ODE建模

1.3 运动控制

(1)D-H参数设计

根据上述手臂模型，采用D-H法建立臂部7-DoF(不含末端夹具)运动学模型。其D-H参数如表1所示，其中：di表示轴的共法线长度，θi表示关节轴对于上一关节轴转角，ai表示连杆的偏移距离，βi表示关节旋转角。运动学计算通过C++调用MatlabRobotic工具箱完成，如图4所示。

表1 7-DoF运动学模型D-H参数表

图4 7-DoF机械手臂运动学模型(不含末端夹具)

(2)基于Kinect的解决手臂冗余算法

Kinect体感设备可采集到人体20个关节点的坐标。其中手臂可以采集到肩关节、肘关节、腕关节和指尖的位置坐标，位置分别为A、B、C、D点，如图5所示。为便于机械手臂动作学习，设计ArmPlant支持Kinect人体动作数据的转化和驱动。

图5 Kinect采集人体运动骨架

(1)

(5)

(8)

具体算法如下：

1)上臂的运动拆分为在Z轴和X轴的两个旋转运动，首先计算出连杆与水平面的夹角，θ为向量AB与Z-X平面之间的夹角,具体求解如公式(9)所示。YB表示B点在A点为原点坐标系Y轴方向下的坐标，XB表示B点在A点为原点坐标系X轴方向下的坐标。据上述的角度解，得到向量AB在初始姿态下绕X轴的旋转角度也就是第一个自由度的旋转角度，具体求解如公式(10)所示。其中i表示所在的象限数，j表示旋转方向，根据位置坐标判断j的正负，在第1、2、5、6象限的时候表示-1，在3、4、7、8象限的时候表示1。

θ=atan(|YB|/|XB|)

(9)

(10)

2)已知第一个旋转角度的基础下，计算自由度2的旋转角度，计算出向量AB和平面Z-Y的夹角，具体求解如公式(11)所示，其中i表示B点所在的象限数。

θ2=(-1)iarctan(((〗1)i|ZB|)*sinθ1/|YB|)

(11)

3)根据向量AB和向量BC利用空间的夹角计算出肘关节的自由度4的夹角，具体求解如公式(12)所示。

(12)

4)根据逆运动学的求解方式，在已知θ1、θ2、θ4的情况，联立如公式(13)(14)所示的方程组解出自由度θ3，在已知θ1、θ2、θ3、θ4的情况，联立如公式(15)(16)所示的方程组解出自由度θ5。其中L2和L3分别表示Kinect采集数据中上臂、前臂长度，T表示逆运动学中的旋转矩阵。

(13)

(14)

(15)

(16)

1.4 手臂驱动

ODE的关节驱动方式包括力矩驱动和角速度驱动，为便于实现机械手臂的控制，本平台对这两种驱动方式都支持。目前，本平台支持的手臂角速度控制方式包括匀速控制和P控制(即比例控制)。此外，为满足实际研发需求，平台增加了控制数据的加载功能，方便开发人员实现手臂动作的个性化控制算法。匀速控制是根据时限、目标角度计算角速度和步长个数，流程图如图6(a)所示。P控制需要在每个步长之前进行差值计算，根据差值设置当前时刻需要的角速度，流程图如图6(b)所示。

2 实验及结果

长期以来，在机械手臂领域中缺乏统一有效的评测场景、尺度及方法，对手臂动作的灵活性、稳定性更是难以评价[1,12-15]。根据轻型仿人机械手臂的技术特点，设定如下实验场景进行评测。

(1)实验一：物体抓取和定点码垛。在刚性适度、结构稳定、灵活性和稳定性都保证前提下，机械手臂才能快速、准确完成物体抓取及定点码垛。实验场景要求在10秒内在两个间隔为0.4 m桌子之间完成3个质量0.02 kg、边长5 cm的方块体抓取和码垛，判断目标位置和实际位置的误差。仿真实验过程如图7所示，结果如表2所示。结果有误差(毫米级)的原因为ODE仿真计算的碰撞检测计算及逆运动学计算中存在一定的误差，但是在可接受范围内。结果表明：仿真中机械手臂可以在设定的环境参数下成功抓取物体并且完成码垛，说明该手臂具有一定的灵活性和稳定性。

图6 支持的2种角速度控制算法

图7 仿真物体抓取及码垛过程

(2)实验二：人体动作的实时跟踪再现。利用Kinect捕捉关节位置，通过前述姿态控制运动学模型计算出关节角度，驱动仿真机械手臂及手臂物理样机对动作的实时跟踪再现，如图8所示，判断肘关节和腕关节的所在位置点的误差。

表2 物体抓取和定点码垛结果

表3给出人体运动再现的仿真实验结果，第2、3列分别是ODE坐标和Kinect坐标下的肘部和腕部坐标。结果表明，肘部关节的误差较小(毫米级)，腕部关节误差稍大，但总体上低于厘米级，能够比较准确地完成人体运动跟踪和再现，从而验证了仿真模型的准确性。

(3)实验三：物体的抓取和码垛。根据上述模型设计并实现一款8-DoF手臂样机，其码垛实验结果如图9所示。3个方块摆放时基本在同一条直线上，有少许误差(厘米级)，原因是电机精度、齿差以及样机零部件加工精度等因素的影响，有待进一步改进。

图8 人体动作跟踪再现仿真结果

动作序号ODE坐标(x,y,z)/mKinect坐标(x,y,z)/m误差/m1肘-0.115,0.224,-0.028-0.110,0.222,-0.0280.005腕-0.217,0.428,0.004-0.180,0.440,-0.0020.0292肘-0.025,-0.250,-0.005-0.024,-0.248,-0.0050.001腕-0.042,-0.480,0.013-0.040,-0.477,0.0130.0033肘-0.037,-0.048,-0.242-0.037,-0.048,-0.2420.001腕-0.118,-0.255,-0.301-0.035,-0.268,-0.3100.0854肘0.002,0.068,0.1250.002,0.065,0.1260.001腕-0.017,0.306,0.126-0.018,0.293,0.1150.018

图9 码垛结果图

3 结论

本文建立了轻型仿人机械手臂的集成开发平台ArmPlant，并以此为基础设计并实现8-DoF轻型仿人机械手臂。实验过程选取物体连续抓取、人体动作跟踪再现这两个场景进行仿真实验及样机实测，对平台功能及性能进行了验证。结果表明，该平台集尺寸设计、质量分布、运动学及逆运动学计算、动作规划和编辑、仿真验证、样机驱动、运动评价等于一体，支持仿人机械手臂的完整研制过程，为轻型仿人机械手臂的研制提供了快速、便捷、功能比较完备的集成开发平台。