基于OpenMV的五自由度机械臂控制方法研究

张宇豪，庞辰骅，宋子韩，钱嘉毅，蒋书波

(南京工业大学电气工程与控制科学学院，江苏南京 211816)

0 前言

工业机器人是具有多关节或多自由度的机器装置，代替人工完成一些繁重复杂甚至危险环境下的工作，它具有一定的感知功能，能够实现对环境和工作对象的自主判断与决策。工业机器人已经广泛应用于生产和生活的各个方面，特别是在物流行业与分拣领域有着极大的研究与应用价值，不仅大量减少了人为操作的时间和精力，还较大程度上提高了生产效率[1]。目前我国工业机器人的应用主要集中在汽车制造产业、3C电子电气产业、金属加工产业、塑料和化学制品产业等多个领域[2]。在排爆、航空航天、放射物环境以及其他高危行业的工程应用中，对移动机械臂视觉伺服系统进行研究有着重要的工程应用价值与广阔的发展前景。

本文作者以此为背景，对机器视觉定位与机械臂的运动控制进行了探索，研究了基于视觉感知的移动机器人控制系统，引导机器人自主地从起始点向目标点运动，完成抓取操作。本文作者建立仿真模型，验证了运动学求解方法；同时完成了硬件搭建以及软件调试，对整套机械臂抓取控制系统的可行性进行了验证。

1 系统总体方案设计

五自由度机械臂比市面上更多自由度的机械臂成本低，相比于四自由度的机械臂可以实现更加精确的操控。本文作者以五自由度机械臂为研究对象，通过视觉传感系统来定位目标物，通过对机械臂逆运动学的求解及运动轨迹的规划控制机械臂。

此五自由度机械臂采用了5个数字金属舵机相连接，其结构如图1所示。舵机的本质是一套小型的闭环伺服系统，它接收目标信号，通过驱动内部的直流电机，并经过减速齿轮组，从而调整输出轴的角度。该输出角度通过电位器采样，再由反馈控制系统调整其输出角度与目标值匹配。整个过程构成一个典型的闭环反馈控制系统。

图1 五自由度机械臂结构

机械臂包括舵机和机械手，舵机包括5个数字舵机。该数字舵机与传统的模拟舵机相比，具有控制精度高、线性度好、响应速度快的优点。第一舵机固定设置在机械臂底座上，第一舵机与第二舵机通过第一支架相连接；第二舵机与第三舵机通过第二支架相连接，第三舵机与第四舵机通过第三支架相连接，第四舵机与第五舵机通过第四支架相连接，第五舵机连接机械手。第一舵机用于控制方向，第五舵机控制机械手的开合，第二、三、四舵机用于控制旋转角度。

此系统利用OpenMV摄像头，完成对目标球位置信息的采集，主控板采用STM32F103处理器，根据位置信息控制五自由度机械臂模块，从而完成对目标球的抓取。

图2表示了一种基于位置反馈的视觉伺服控制系统框图，首先从相机拍摄到的图片获取目标物位姿，接着将其反馈到输入端，与期望位姿比较得到偏差，并根据偏差值驱动关节控制器，最后驱动机械臂运动完成工作任务。

图2 基于位置的视觉伺服控制系统

1.1 系统硬件结构设计

系统硬件结构如图3所示：OpenMV摄像头采集图像信息，采集到的图像信息通过 A/D 转换，生成数字图像，采用图像识别与处理算法对这些数字图像进行辨识[3]，并得到识别结果。如若符合乒乓球的特征即为识别成功，然后摄像头通过串口通信发送指令到STM32F103主控板；主控板对机械臂的运动姿态进行控制，确定每个舵机转动的角度，规划出最优运动路径，最终完成抓取球的动作。

图3 系统硬件结构

1.2 控制程序设计

图4为系统控制主程序的流程，机器人各部分装置协调有序运行，互不干扰，保证程序运行的安全。主要实施过程为寻物、移动和抓取，首先对目标物进行识别与定位，接着机器人根据目标物的位置信息调整位姿，确保目标物在机械臂手爪的工作范围内。

图4 机械臂运动控制主程序的流程

下一步要实现的是将视觉感知目标物与机械臂操作相结合，图5为机械臂的程序框图。STM32F103主控板接收目标物的位置信息，包括经过数据处理后的角度信息，即视觉检测装置根据目标物的位置采用几何法求逆解得到的机械臂各个关节的转动角度。由此，便得到机械臂起始与终止时刻的姿态，使用五次多项式插值法对机械臂的运动轨迹进行规划，引导机器人从初始点向目标点运动。在完成抓取动作过程中，需要根据实际情况来确定各个关节舵机的速度快慢，或者插入中间运动姿态来规避机械臂不能到达的姿态，使机械臂的运动轨迹更加平滑，可靠性更高。

图5 机械臂程序框图

2 机械臂运动学分析

2.1 运动学模型建立

运动学建模分析是实现机械臂运动轨迹规划等后续工作的基础。机械臂最常用的建模方法是D-H模型法，其基本思想为：为机械臂的每一连杆建立一个坐标系，用齐次变换矩阵来表示相邻关节的位姿，然后获得末端执行器在基坐标系下的位姿[4]。图6所示为相邻关节D-H模型示意。

图6 D-H模型示意

图6中设连杆i的坐标系为{i}，Zi轴与轴i同向，原点位于轴i、i+1的公垂线与轴i的交点处，Xi轴沿着公垂线由轴i指向轴i+1，Yi轴由右手定则确定。其中，4个参数αi-1、ai-1、θi、di为连杆i的D-H参数，描述了机械臂关节和连杆之间位置关系，其物理含义如表1所示。

表1 机械臂D-H参数

本文作者以五自由度机械臂为对象，根据D-H模型法在各个连杆建立合适的坐标系，如图7所示。其中，关节J1逆时针转过的角度记为-θ1，关节J1与关节J2的连杆长度为L1，以此类推。

图7 机械臂模型结构

表2为机械臂的建模参数。

表2 建模参数

2.2 机械臂正运动学分析

(1)

(2)

(3)

其中：

(4)

2.3 机械臂逆运动学分析

逆运动学分析是已知末端位姿反解出各关节角度，是机械臂控制中比较重要的分析方法[5]。逆运动学的求解比正运动学复杂得多，求解方法主要包括代数法、几何法、迭代法等。本文作者使用代数法和几何法来对逆运动学求解进行对比分析。

2.3.1 代数法求逆解

(5)

(6)

令等式(6)两端元素(2，4)对应相等，得：

qy·cθ1-qx·sθ1=-L1

(7)

求解得到θ1为

(8)

在已知θ1的情况下，可依次计算各关节角度值为

(9)

其中：

(10)

经过分析可知，代数法计算的关节角度存在多解的情况。在实际情况中，由于关节结构等限制，需要根据实际情况对旋转角度的具体数值进行取舍，最终得到一组最优解。

2.3.2 几何法求逆解

代数法在求解过程中代数较多，计算量较大，求逆也比较复杂，不利于机械臂的实时控制。本文作者使用一种基于机械臂结构几何特征提取推导求解的方法。

假设机械臂抓取时的姿态是唯一确定的，无论5个关节角度变量取何值，连杆L1、L2、L3、L4的运动始终处于同一平面，平面的位置仅由θ1确定。选取该二维平面下的机械臂结构进行分析，如图8所示。

图8 二维平面下机械臂结构

规定a为目标物到机械臂底座的水平距离，b为J4距地面的高度，c为J1距地面的高度，L5为J5与目标物中心的水平距离。根据图8所示的机械臂结构的几何关系，可得关节转动角度计算公式为

(11)

求解得到：

(12)

(13)

θ4=π/2-θ2-θ3

(14)

将目标点在三维空间下的坐标映射到基坐标系中xOy平面得到点(x,y)，根据几何关系可以直接求得：

θ1=arctan(y/x)

(15)

上述研究表明，利用几何方法求5个自由度机器人逆解的速度快，效率高，推导过程简单直观。但是，由于几何方法不具有通用性，且没有一个系统的解法，因此，在多自由度机器人的运动过程中，采用几何法求解不仅计算困难，而且效率低下。

2.4 轨迹规划

经过逆运动学求解得到机械臂各关节角度后，可以确定终止时刻机械臂的姿态。轨迹规划目的是使机械臂从初始时刻至终止时刻的运动轨迹平滑，各关节不发生急剧的速度变化，不产生强烈的冲击。在机械臂关节空间中以关节角函数的形式对运动轨迹进行描述，其中多项式插值法是一种经典的机械臂轨迹的插值计算方法[6]。

如果已知机械臂初始时刻t=0和终止时刻t=tf的关节位置、角速度和角加速度，可以采用五次多项式对关节轨迹进行插值：

θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(16)

关节角度、角速度以及角加速度的边界条件为

(17)

求解得到各项系数为

(18)

根据边界条件可以唯一确定五次多项式插值轨迹，保证机械臂关节角度、角速度、角加速度曲线平滑连续。使用五次多项式插值法进行轨迹规划。

3 图像识别与处理

3.1 图像特征选取

选取合适的图像特征，可以减少视觉伺服控制算法的复杂性，以获得更好的控制效果。阈值分割法是图像分割的一种经典方法[7]，它利用要提取的目标与背景在灰度上的差异，通过设定阈值对图像进行分组聚类，将图像分成若干个特定的有意义的区域，目的是把目标从背景中分离出来，便于图像分析[8]。用数学表达式来表示，设原始图像为f(x,y)，T为设定的阈值，则经阈值分割后的图像g(x,y)定义[9]为

(19)

式(19)中：标记为1的像素对应于目标，标记为0的像素对应于背景。摄像头采集的原图像和原图二值化图像的背景干扰噪声较大，分割效果不理想，直接识别目标球非常困难。

3.2 图像处理

为获得高质量图像，需要对图像做进一步处理：

(1)双边滤波。双边滤波是一种非线性的图像滤波方法，它是一种结合图像的空间邻近度和像素值相似度的折中处理，同时考虑空间信息和灰度相似性，可以很好地保存边缘，达到去噪的目的。公式(20)给出了双边滤波的操作[10-11]：

(20)

(21)

图9(a)(b)分别展示了双边滤波后的彩图与二值化图像，由于保存了过多的高频信息，双边滤波器不能够干净地滤除彩色图像里的高频噪声，只能够较好地过滤低频信息。

图9 图像处理效果

(2)腐蚀与膨胀。腐蚀与膨胀是最基本的形态学操作[12-13]，主要利用图像中干扰像素与目标物所占面积的巨大差异，通过腐蚀操作消除物体的边界点，使边界向内部收缩，再经膨胀操作后还原对象的基本特征，使边界向外扩张。腐蚀的数学表达式为

S=X⊗B={x,y|Bxy⊆X}

(22)

式中：S表示腐蚀后的二值图像；B表示用来进行腐蚀操作的结构元素；X表示原图经过二值化后的像素集合。腐蚀的数学表达式为

S=X⊕B={x,y|Bxy∩X≠φ}

(23)

式中：S表示膨胀后的二值图像；B表示用来进行膨胀操作的结构元素；X表示原图经过二值化后的像素集合。图9(c)将双边滤波后的二值化图像进行腐蚀与膨胀操作来使图像保持边缘，降噪平滑。

(3)设定面积阈值。图9(d)设定一定大小的面积阈值来过滤掉较大的色块，即背景干扰的部分，通过轮廓检测识别出距离最近的乒乓球，并用矩形框出。

3.3 OpenMV程序设计

OpenMV主控为STM32H7，主频为 480 MHz，RAM 为1 MB，使用灵活便捷的 Python语言，在其专有的 OpenMV IDE 开发环境下调用基于视觉的图像识别与处理算法[14]。图10为OpenMV程序流程，设计涉及的颜色识别是计算机端通过颜色阈值编辑器选取目标物的最佳阈值，然后按照目标物的颜色进行阈值分割，得到二值化图像。图像处理算法主要基于二值化图像进行滤波处理、形态学处理以及轮廓检测得到目标色块，从而提高识别目标物的准确度。

图10 OpenMV程序流程

4 实验及结果

搭建实验硬件时，摄像头固定在机器人后方，安装在距地面45 cm高度处，并与竖直碳素杆的角度为30°；机械臂安装在机器人中间靠前部的位置，能使机械臂的运动不与机器人碰撞。各功能模块装置在合适位置，保证系统模块互不干扰、协调工作。经过测试，智能抓取机器人能够准确识别到目标球并抓取回收。

从表3可以看出：二维图像坐标(x，y)理论值与实际值相差很小，平均定位偏差为1.29 mm，具有较高定位精度；目标物与机器人之间的距离理论值与实际值存在偏差，最大相对误差为4.87%，最小相对误差为1.46%，平均相对误差为2.87%，满足机器人的抓取要求。经分析，定位偏差与距离偏差来源于相机实际参数的影响，在坐标变换时产生误差。

表3 视觉部分理论值与实际值

对拾取球的成功率进行测试，测试结果如表4所示：能够拾取大多数的球，成功率较高，但还是会受到光照强度、球密度、球位置等环境因素的影响。

表4 测试结果

除上述环境影响因素外，PWM占空比的精度也会使实验结果产生误差。舵机从0°到270°不同角度的控制是通过改变占空比实现的，占空比精度越高，控制舵机角度更精准[15]。在一个脉冲周期内，若占空比的精度只有1%，它只能产生 30%、56%这种整数的占空比，不能产生26.5%这种占空比。

5 结语

在机械臂运动学分析与轨迹规划的研究中，作者使用代数法与几何法对机械臂的逆运动学求解进行对比分析。同时，在系统设计方案方面，给出了各个模块的硬件选型与功能介绍。为了精确定位目标物，分析了图像识别与处理方法。在系统控制程序设计中，选取几何法求机械臂运动学逆解。结合OpenMV的图像识别与处理算法、智能小车的路径规划算法，主控板通过PWM信号控制舵机的机械臂从而控制系统精准地定位并抓取目标物，对整套机械臂抓取控制系统的可行性进行了验证。