Dobot机械臂运动轨迹规划与控制

2022-05-12唐雯雯王明芬

机电工程技术 2022年3期

唐雯雯，卢宇，王明芬

（福建师范大学协和学院，福州 350117）

0 引言

随着工业自动化的发展，机械臂在产业自动化方面已经得到了相当广泛地应用。机械臂在复杂、枯燥甚至是恶劣环境下，无论是完成效率以及完成精确性都是人类所无法比拟的，也因此，机械臂在人类的生产和生活中发挥着越来越重要的作用。机械臂可替代人的繁重劳动以实现生产的机械化和自动化，能在有害环境下操作以保护人身安全，因而广泛用于机械制造、冶金、电子、轻工和原子能等行业。针对目前机器人在各个领域的应用情况，机器人的运动规划控制已经成为了一个重要的研究方向，并具有极大的实用经济价值。但机械臂的控制精度、响应时间、系统鲁棒性、使用寿命、维护周期等问题都亟待解决。目前国内外学者对机械臂控制的研究有很多，但对Dobot Magician这种高精度轻量型智能实训机械臂的研究几乎没有，因此本文对于Dobot机械臂的研究开发具有较高的市场价值。

本文旨在研究Dobot机械臂的轨迹规划问题。首先通过建立机械臂模型并进行正运动学方程求解，再采用数值法确定机械臂在空间中的活动范围，使机械臂能够更好地抓取目标物体；然后利用五次多项式插值法对Dobot机械臂进行运动轨迹规划。该研究依据Dobot机械臂高精度且轻量的性能特点，设计了一步步功能测试实验，开展了对Dobot机械臂控制方案的研究[1]。

1 Dobot机械臂正运动学分析

1.1 连杆坐标系的建立

Dobot机械臂是一种四自由度串联开链式机械臂。为了描述机械臂各连杆之间的相对位置和方向关系，利用Denavit-Hartenberg参数建模法建立了关节坐标系[2]。其分布情况如图1～2所示。针对Dobot Magician机械臂的连杆机构，其D-H参数如表1所示。

图1 机械臂D-H建模连杆坐标系

图2 连杆关节

表1 D-H参数

1.2 Dobot正运动学求解

正运动学求解是指以Dobot机械臂各个关节变量数值为已知条件，通过齐次变换方程正向求解其末端在空间中的位姿[3]，为了表示方便，令：

由D-H转换法则，得各坐标系转换矩阵分别为

1.3 Dobot机械臂运动轨迹规划

在关节空间中，这里使用五次多项式插值法进行Dobot机械臂运动轨迹规划的研究。其中需要6个约束条件，包括运动段起点和终点的角度、角速度及角加速度。已知机器人某一关节i在初始时刻t0的初始角度为θi(0)，终止时刻tf终止角度θi(tf)。假设这6个约束条件分别为：

设由这6个约束条件唯一确定的五次多项式的形式为：

其中，系数a0，a1，...，a5必须满足以下6个条件，即：

将6个已知的约束条件代入方程，联立解得：

因此，由这6个已知条件，可求得五次多项式的系数，也就是求得了第i关节在每一时刻的运动角度、角速度以及角加速度，即得到了第i关节的运动轨迹[4]。

2 Matlab仿真

2.1 正运动学结果验证

在Matlab环境下，借助机械臂工具箱Robotics Toolbox建立了四自由度机械臂模型。4个参数值共同确定了末端执行器的位姿。通过驱动图对应的4个滑块即改变了机械臂末端执行器的位置。Dobot初始姿态如图3所示，当机械臂关节角q1=0，q2=0，q3=0，q4=0时，机器人末端位置为［343.05 0143.0］，单位为cm，X-Y-Z固定角坐标系的回转角、俯仰角和偏转角分别为[0 0 90]，单位为°[5]。

图3 初始位姿仿真

经正运动学求解初始姿态为：

Dobot末端姿态如图4所示，当机械臂关节角q1=π∕6，q2=π∕6，q3=π∕6，q4=π∕6时，机器人末端位置为[154.048，111.922，112.856]，单位为cm，X-Y-Z固定角坐标系的回转角、俯仰角和偏转角分别为[90-36-90]，单位为°。经正运动学求解初始姿态为[6]：

图4 末端位置仿真

矩阵演算结果中的姿态矩阵和位置矩阵均与Matlab中Dobot机械臂仿真模型的结果一致，从而验证了正运动学分析的正确性[7]。

2.2 关节空间规划

假设各个关节运动时间间隔为5 s，各个关节初始和终止的角速度都为0，运动时间为40 s；利用五次多项式插值法的jtraj函数进行轨迹规划，分别得到了机械臂从p1到p2各个关节的角度、角速度及角加速度与时间的关系曲线[8]。仿真程序如图5所示。

图5 仿真程序

矩阵q每一行表示在此时间采样点上各关节的关节角度；qd和qdd每一行表示相应关节的角速度与角加速度，初始和终止时刻的角速度及角加速度默认为0。init_ang和targ_ang分别表示机械臂各关节初始位置角度和终止位置角度；分别绘制了各个关节的角度、角速度及角加速度随时间变化的曲线。由图6易知，各关节角度值从起点到终点的变化曲线呈现五次多项式曲线变化，验证了仿真参数的合理性和有效性。

图6 机械臂末端执行器坐标变化曲线

3 Dobot机械臂实验研究

3.1 实验研究方案

本研究通过对实体机械臂的控制接口及通信协议的分析，用python语言实现了Dobot机械臂的二次开发。由于Dobot机械臂底层代码并不开源，进行二次开发、控制算法相关实验研究时通过串口通信技术来接入PC机板控制器利用通信协议和Dobot控制器进行信息交互，对机械臂的运动控制进行了实验研究[9]。

3.2 实验硬件和软件

此实验硬件由计算机、Dobot机械臂、控制器、传感器等组成整个控制算法实验系统。上位机运行Visual Studio Code软件，pyQt提供了Dobot各项功能的操作界面（图7）。上位机与Dobot控制器建立了串口通讯、提供了用户操作界面。Dobot控制器接收到上位机发送的控制命令，与机械臂之间建立了位置闭环控制，从而驱动Dobot机械臂的运动。

图7 机械臂控制界面

3.3 点动功能研究

为深刻理解机械臂的轨迹规划以及各部分的运动关系，在利用Matlab仿真运动之后，规划了机械臂的末端沿设定的特定轨迹运动过程。首先输入指令机械臂移动到指定坐标（x，y，z），并且获取机械臂姿态检测是否移动到（x，y，z），试验结果显示控制算法效果较为理想，机械臂可到达运动范围内任意指定的位置。

图8 机械臂运动回零位置

图9 机械臂运动终点位置

3.4 圆弧功能研究

圆弧实验步骤如下。

（1）准备工作。打开电源，检查机械臂指示灯，若是绿色则能正常工作。

（2）开启下位机，开启上位机控制软件运行系统，并启动画画模式。

（3）执行回零功能，如图10所示。为保证在工作范围内完成绘画，选择合适的姿态落笔到纸上。如图11所示。

图10 机械臂回零

图11 调整机械臂笔尖位置

（4）设置画画的速度，打开pyCharm软件，配置加速度选项，根据快慢调节，推荐参数50，输入笔下压的距离2 mm。运行绘画设计程序，机械臂将沿着给定的期望位置运动，画画效果如图12所示。机械臂最终的绘画效果图的关键位置点均与设想位置基本一致，在图画轮廓的转折处变化相对平滑。而绘画过程中接触力不好控制，这一点仍需改进[10]。

图12 绘画效果

3.5 对弈功能研究

3.5.1 实验步骤

（1）找到游戏板的顶点。

（2）让Dobot画"O"和"X"两种标记。

（3）创建游戏环境。

（4）实施搜索算法。

（5）实现状态差分。

（6）运动检测的实现。

（7）处理相机图片以获得玩家的动作。

（8）处理当前的状态，以找到最佳棋步[11]。

3.5.2 软件设计原理

首先系统通过图像处理技术校准棋盘，然后辨别两种棋子的形状，将棋子位置传给求解函数并返回棋盘状态，从而定位棋子位置，提取出当前棋局信息。设置玩家下棋过程的10 s倒计时，从差异最大的区域捕捉玩家的下棋动作。程序从当前状态需要模拟了每个可能的动作，为了推进游戏的状态，使用搜索算法实现了游戏中机械臂下棋策略的分析。该算法不断比较一个启发式数，作为游戏的结果。最终通过PC机发送相应的指令控制机械臂执行落子动作[12]。