虚实结合的低成本开源机器人实验教学平台

谭彩铭， 张 成， 陈 旭， 梁志伟， 高 翔

（南京邮电大学a.自动化学院、人工智能学院；b.机器人信息感知与控制研究所，南京 210023）

0 引言

机器人技术具有理论与实践紧密结合的特点，实验教学是机器人教学中的重要环节。常见的机器人实验教学平台采用ABB、FANUC、KUKA 等品牌的机器人，其产品性能优良，贴近工业现场，但机械结构封闭，系统开放性差，且成本高、占地大、维护复杂［1-2］；单纯采用计算机仿真技术性价比高、安全性好、使用方便，但仿真与实体机器人的开发与调试方法有很大区别［3-4］；采用仿真与实体机器人相结合即虚实结合的方式，可以一定程度融合两者的优势，但仍然存在品牌机器人开放性差、占地大等不足［5-6］。在教学活动中发现，学生在使用品牌机器人的时候，即便采用虚实结合的方式，仍存在以下几点突出问题：①仅能在实验室开放时间使用；②由于设备价格昂贵，为避免损坏，学生不敢轻易尝试；③人均台套数低，需要排队使用；④需要另外学习专用的机器人仿真软件，增加了学习负担与入门难度。这些大大制约了学生的学习研究进程。

基于此，本文将虚实结合方案与低成本开源方案相结合，设计并实现基于Matlab 和低成本开源机械臂的机器人实验教学平台，充分发挥仿真与实体机器人的优势，弥补品牌机器人开放性差、成本高、占地大、维护难等缺陷。Matlab是系统仿真和自动控制领域普遍采用的计算机辅助设计工具，不少专业还开设Matlab课程［7］，基于Matlab编写的Robotics 工具箱大大提升了Matlab机器人编程的效率［8-9］。低成本开源机械臂由6 个舵机、若干金属支架和手爪组成，质量约1 kg，最大臂展接近0.5 m。通过逐步实现机械臂抓取增强学生对机械臂驱动、建模、正逆运动学、轨迹规划、视觉感知、手眼标定等机器人学中关键概念的理解。设计基于单个定时器的6 舵机同步驱动方法，避免了对单片机PWM模块和定时器模块数量的依赖；基于标志点的手眼标定方法，使得‘手’、‘眼’移位后也能快速让‘眼’正确感知；基于几何法的逆运动学快速计算方法，解决常规逆运动学方法对5 自由度机械臂不适用问题。通过这些改进，进一步提高运行效率降低经济成本。实验结果显示，Matlab 仿真机械臂与实物机械臂同步完成对工件的抓取。

1 平台总体设计方案

实验平台构成如图1 所示。下位机由单片机构成，实现舵机驱动以及与上位机通信。双目相机用于感知，增加机械臂抓取的自动化程度。上位机使用Matlab编写双目相机测量与机械臂抓取控制程序。

图1 机器人实验教学平台构成

基于该平台实现机械臂抓取的总体方案流程如图2 所示，其关键流程包括机械臂驱动、机械臂运动学建模、手眼标定和机械臂抓取4 个部分，涵盖了机器人学主要知识点。

图2 平台机械臂抓取总体方案流程

2 机械臂驱动设计

舵机是一种位置（角度）伺服驱动器，其控制信号为PWM波，周期为20 ms，通过改变脉宽来改变舵机角度，脉宽和舵机角度之间呈线性关系，脉宽在0.5 ～2.5 ms 范围［10］。开源机械臂机械手部分采用TBSNK15 舵机，角度变化为0° ～180°，其他5 个舵机均为TBS2701，角度变化为-45° ～225°。这两种型号的舵机角度与脉宽的控制关系见表1。基于该开源机械臂单片机，设计并实现单个定时器的6 舵机同步PWM波。

表1 脉宽与所用舵机角度之间的控制关系

2.1 单片机硬件电路

如图3 所示，该开源机械臂驱动电路采用IAP15W4K61S4 型STC 单片机，以IAP 开头型号的单片机本身就是仿真器，可直接在线仿真，方便学生调试和下载程序。STC 单片机对之前的51 内核进行了重大改良并增加了很多片内外设，其性能超越了AT89系列51 单片机。现在电脑普遍不安装串口，使用USB转串口芯片CH341，把电脑的USB 口映射为串口使用，通过该接口实现上位机和下位机之间的通信以及单片机程序下载和在线调试。6 个舵机的控制信号分别由单片机的6 个I/O引脚提供。另外按键和指示灯用于人机交互和状态显示，扩展接口用于外接其他设备。

图3 驱动开源机械臂的单片机硬件框图

2.2 基于单个定时器的6 舵机同步驱动程序

使用定时器中断来产生PWM 波。设第i个舵机（记为Si）的脉宽为wi，由表1 可知，0.5 ms≤wi≤2.5 ms。如图4 所示，将整个20 ms 的周期分为8 个2.5ms的小时间段，令S1对应的I/O 引脚输出置为高电平，并设置定时周期为w1，定时周期到触发定时中断，将S1对应的I/O引脚输出置为低电平，并设置定时周期为2.5 ms-w1，定时周期到触发定时中断，将S2对应的I/ O 引脚输出置为高电平，并设置定时周期为w2，以此类推，其中在将最后一个S6对应的I/O 引脚输出置为低电平后，设置定时周期为7.5 ms -w6，这样6 个舵机的一次控制周期之和为20 ms，循环往复，这样就在6 个I/O引脚上分别产生6 个周期为20 ms，脉宽时间不同的PWM波。以上过程，每20 ms的周期里一共有12 次定时器中断，在任意时刻，当某个舵机输入为高电平时，其他舵机输入必定是低电平。这里将I/O引脚设置为强推挽输出以驱动舵机。

图4 基于单个定时器的6舵机同步驱动PWM波产生原理示意图

为使机械臂运动更加平滑，在当前脉宽和目标脉宽之间按固定步长插补若干均匀递增或递减的脉宽，依次输入如图4 所示的PWM 波产生函数，这个步长和机械臂运动速度对应。除使用定时器外，也可直接使用单片机内嵌的PWM模块产生PWM信号。

3 机械臂运动学建模

对机械臂进行运动学建模是进行正、逆运动学计算、轨迹规划等过程的基础。

3.1 DH建模

针对开源机械臂，采用Denavit Hartenber 提出的DH方法定义连杆坐标系［11］，各连杆坐标系定义如图5所示（不包括末端执行器）。其中坐标系0 作为基座坐标系，相对桌面固定。坐标系5 通常被称作末端执行器坐标系，将其原点设置在机械手末端的两个手指中间。坐标系i到坐标系i-1 的齐次变换矩阵为

图5 开源机械臂连杆坐标系定义

上式对应的物理变换过程：

（1）沿zi-1轴旋转θi角，使xi-1轴与xi轴平行。（2）沿zi-1轴平移距离di，进一步将xi-1轴与xi轴共线。

（3）沿xi-1轴平移距离ai，使坐标系i-1 原点与坐标系i的原点重合。

（4）沿xi-1轴旋转αi角，使zi-1轴与zi轴共线［12］。

该开源机械臂DH参数见表2。

表2 开源机械臂DH参数

使用Robotics工具箱9.10 版本Link 函数建立每个连杆对象，即调用

式中，sigma 为指定旋转或平移关节类型。使用SerialLink函数建立串联机器人对象，即调用

式中，L为按序包含串联机器人连杆的数组。

3.2 关节转角数据发送

如图5 所示，机械臂处于竖直位置时，所有舵机处于中间位置，即舵机的角度ϑi处于表1 所示的90°。

Matlab仿真机械臂的关节转角θi和实物机械臂的关节转角ϑi之间存在如下对应关系

由表1 可知，舵机TBS2701 脉宽tH和舵机角度ϑ之间的关系满足

式中，tH的单位是μs；舵机角度ϑ的单位是（°）。

Matlab提供对系统串口的支持。运行

scom= serial（‘COM3’，‘BaudRate’，115200）

建立一个串口对象scom，这里串口号为COM3，波特率为115200，运行fopen（scom）打开串口，运行fprintf（scom，str）以文本形式向串口发送一帧数据str，帧格式示例为“#1P1500#2P1500#3P1500#4P1500#5P1500#6P1500”，字符‘#’与‘P’之间的数字表示舵机号，字符‘P’后面紧跟的数据1500 为该号舵机的脉宽，可以是500 ～2500 之间的任意数据，单位为μs。下位机接收到数据后，驱动所有舵机按指令转动。

4 基于标志点的手眼标定

手眼标定是使双目相机感知的定位信息被机械臂正确感知的必要环节［13］。基于标志点的手眼标定方法原理，在机械臂末端固定一个标记点，操纵机械臂运动到若干位置，通过正向运动学获得该标记点在机械臂基座坐标系下的坐标，通过双目相机得到这些位置处标记点在相机坐标系下的坐标，根据迭代最近点（Iterative Closest Points，ICP）算法求得相机坐标系到基座坐标系下的齐次变换矩阵，实现手眼标定，为后续抓取做准备。

4.1 基于双目相机的标志点定位

使用的双目相机由左、右两个水平放置的相机组成，双目相机的光轴平行、相机参数一致。如图6 所示，利用针孔模型对相机进行建模，现实世界的空间点光线经过光心之后，落在物理成像平面，形成成像点，物理成像平面到光心的距离为焦距f。左、右两个相机的光心之间的距离称为双目相机的基线，记为b。设空间点P在左、右眼的成像点分别为PL和PR，PL和PR在各自的图像物理坐标系的坐标分别为（xL，yL）和（xR，yR）。

图6 双目相机的成像模型

定义双目相机的坐标系O-xcyczc为右眼相机坐标系，P点在相机坐标系z轴坐标即为深度Z，根据三角形相似关系，可得

整理得

在得到深度的情况下，根据三角形相似关系可进一步得到P点在相机坐标系O-xcyczc下的坐标，用向量表示，满足

相机内部传感器在成像平面上对像进行采样和量化，将光线转换成图像像素。实际通过相机最终获得的是一个个像素，每个像素对应的物理尺寸是相机的固有参数，通过该参数可得像素对应的成像点在图像物理坐标系下的坐标［14］，进而可由式（1）、（2）得空间点在相机坐标系下的坐标。

4.2 基于ICP算法的手眼相对位姿确定

ICP算法是一种点集对点集的配准算法，多用于点云匹配［15］，将其引入手眼标定计算。操纵机械臂，使与连杆5 固定的标记点到达第i个点，该点记为Pi，通过正向运动学得到该点Pi在机械臂基座坐标系0系下的坐标，记为。通过双目相机定位得点Pi在相机坐标系c系下的坐标，记为。表示c系原点在0 系下的坐标表示c系到0 系的旋转矩阵。根据ICP算法，通过对下式取最小求解旋转矩阵和平移向这两个参数，即

其中，n为标记点到达的位置数。采用文献［15］中所述的奇异值分解方法求得和，坐标系c系到0 系的齐次变换矩阵为

式中，01×3=［00 0］T。

5 机械臂抓取

根据手眼标定可得相机坐标系和机械臂基座坐标系的齐次变换矩阵，由双目相机定位可得被抓取目标在相机坐标系下的坐标，经坐标变换可得被抓取目标在基座坐标系下的坐标。控制机械臂自主运动，使机械手顺利抓取目标，该过程主要涉及逆运动学和轨迹规划两个过程。

5.1 基于几何法的逆向运动学

逆向运动学是给定机器人末端执行器相对基座坐标系的期望位姿，求机器人能够到达预期位姿的关节变量［16］。对于5 自由度机械臂（不包括末端执行器），由于其自由度数不足6，不能以任意姿态逼近同一目标位置，会出现逆运动学解不存在的情况。根据机械臂抓取物体的实际过程，提出一种基于几何方法的运动学逆解快速计算方法。如图7 所示，坐标系5 的原点O5在基座坐标系下的坐标用极坐标和高度来描述，其极坐标长度记为r，极坐标角度记为ψ，高度记为h。

图7 基于几何法的开源机械臂运动学逆解求解原理示意图

显然，ψ由关节转角θ1唯一确定，抓取过程中，令机械手平面与竖直方向保持60°去抓取物体，这样关节转角θ5确定，且坐标系3 的原点O3在基座坐标系下的坐标确定，O3高度和极坐标长度分别为

O1O3长度的确定，由于O1O2和O2O3长度确定，根据余弦定理求得ΔO1O2O3和ΔO1AO3各内角的大小，可得∠O2O1A、∠O1O2O3和∠O2O3O4。根据DH模型对关节转角的定义，取其中一组解：θ1= ψ，θ2=∠O2O1A，θ3=∠O1O2O3-π，θ4=π-∠O2O3O4，θ5= -π/2。要使得存在运动学逆解，需满足O1O3≤O1O2+O2O3。

5.2 轨迹规划

抓取过程中，令机械臂末端依次经过以下7 个位置：初始位置、第1 抬起位置、目标位置上方、目标位置（此处控制机械手夹取）、第2 抬起位置、放置位置上方、放置位置（此处控制机械手放开）。

对抓取过程中依次经过的7 个位置，根据5.1 节所述逆向运动学算法计算得到关节角度，再进行关节空间轨迹规划得到相邻位置间的关节角度序列。在Matlab编程时，使用jtrag函数进行关节空间下轨迹规划，调用

式中：q0 为初始关节角度；qf为终止关节角度；step 为过程包含的时间步数。该函数使用五次多项式进行插值规划轨迹，默认初始和终止时关节速度和加速度的值均为零。

6 实验验证

图8 所示为实验场景。将机械臂和双目相机固定在桌面。双目相机内参数已知，首先进行手眼标定，在机械臂连杆5 固定一个白色标记点，基于Matlab 通过串口发送关节角度数据，控制机械手到达7 个位置，根据第4 节说述的手眼标定方法获得相机坐标系到机械臂基座坐标系的齐次变换矩阵。桌面上有一工件，通过双目相机对工件进行定位，代入得到该工件在机械臂基座坐标系0 系下的坐标。根据第5 节所述的机械臂抓取方法对工件进行抓取，轨迹规划所得关节角度数据同时发送给Matlab 仿真机械臂和实物机械臂，控制两者同步运动。如图9、10 所示，二者的运动轨迹一致，实物机械臂成功地抓取工件，其中图10 绘制了仿真机械臂末端的运行轨迹。

图8 实验平台机械臂抓取实验场景

图9 实物机械臂抓取过程

图10 仿真机械臂抓取过程

在实验时，双目相机捕获白色标记点和工件是通过人工鼠标点击得到的，由于工件体积较大，捕获的图像上工件的位置会影响最终定位准确度。视觉感知方法上还可以持续改进，如进行标记点与工件的自动识别、工件三维建模等，进一步提高抓取的智能化程度。

7 结语

本文设计了一种虚实结合的低成本开源机器人实验教学平台，基于Matlab软件建立仿真机械臂，通过一系列流程产生机械臂运动数据，控制开源机械臂和仿真机械臂运动，实验结果显示，仿真机械臂和机械臂运动一致，成功实现对工件的抓取。通过将虚实结合方案与开源方案有机结合，充分发挥仿真与机器人的优势，弥补品牌机器人开放性差、成本高、占地大、维护难等缺陷。此外，特别设计了单个定时器的6 舵机同步驱动程序、标志点的手眼标定方法以及几何法的快速逆运动学解算方法，进一步提高运行效率降低了经济成本。

通过将Matlab 和低成本开源机械臂结合，使学生在Matlab编程时具有针对性，促进学生对机器人学的真正理解，比如使用Link函数建模时，必须要理解DH模型的含义，才能准确建模并模拟实物机械臂。通过模拟工业场景常见的机械臂抓取过程，明确正逆运动学、轨迹规划、手眼标定等是实现机械臂抓取不可或缺的环节，加深学生对机器人学相关概念的理解。

该平台可开展的机器人学实验包括DH 建模、正逆运动学、轨迹规划、单片机编程、双目相机定位、手眼标定、机械臂抓取等。该平台也可用于设计开放性实验，如自主设计正逆运动学算法，对复杂形态的物体进行抓取，进行目标物体识别以便进一步提高机械臂抓取智能化。