基于SSD的码垛机械臂关节速度纠偏运动规划

2021-11-17王清清

计算机仿真 2021年7期

王清清，奚琳

(1.安徽信息工程学院机械工程学院，安徽芜湖 241000；2.安徽工程大学机械工程学院，安徽芜湖 241000)

1 引言

随着全球经济的蓬勃发展，人们对产品的精度要求也越来越高，加上人工成本的不断提升，机器人加工已成为大中型企业首选的加工方式[1-3]。随着机器人的逐渐兴起，码垛成为包装过程中必不可少的流程，是物流自动化技术的新兴技术。码垛作业在现企业物流管理中占据十分重要的地位。早在80年代初期，一些工业发达的国家已经在自动化、机械化码垛方面取得了较好的发展，实现自动化码垛能够提升物流速度，保障员工安全，减少物料浪费[4]。为了确保码垛机械臂保质保量完成工作任务，机械臂需要根据一定运动轨迹进行操作，轨迹规划是码垛机械臂完成任务和运行性能的基础[5]。为此相关研究人员进行了很多研究，并取得了一定成果。

余亮等人[6]提出研究局部闭链码垛机器人运动学分析及轨迹规划方法。该方法利用D-H法获取局部闭链码垛机器人运动学正逆解，根据任务需要，通过可达工作空间完成码垛机器人路径规划，采取将规划结果与运动学逆解相结合的方式，确定机器人各关节位移和速度之间的时变关系，达到缩短运行周期，提升工作效率，但该方法针对码垛机械臂关节速度纠偏存在的影响因素分析甚少，存在一定局限性。张玲[7]提出基于三次样条曲线的码垛机器人平滑轨迹规划方法。针对码垛机器人运行轨迹，采用三次样条曲线进行规划，将轨迹曲线的参数连续性进行结合，实现码垛机器人平滑运行轨迹，保证其工作过程中的运动平稳性和连续性。该方法可有效控制码垛机器人的稳定运行，但该方法在遇到障碍物时壁障性能较差。

基于此，本文提出基于SSD的码垛机械臂关机速度纠偏运动规划方法。借助SSD算法对研究目标进行自动识别，并通过利用直流电机PWM纠偏调速控制模型和PID纠偏控制算法有效实现速度偏差的校正。与传统方法相比，本文方法有效提升码垛机械臂关节运动精度和平稳性，成功实现任务的轨迹规划。

2 码垛机械臂关节速度纠偏运动规划

2.1 基于SSD算法的目标识别

SSD算法通过不同比例候选框和特征图预测目标物体，利用全卷积代替全连接层，有效提升算法精度和速度[8]。正样本为准确与真实框相匹配的设定，负样本为其它框设定，其位置、置信度和反向传播三种损失的加权总和为算法的损失值，网络结构如图1所示。

图1 SSD网络结构图

以VGG 16为算法基准网络，将该全连接层转变成卷积层。对参数为2×2的最大池化层转变为3×3的最大池化，池化层的转变能够带动感受野发生改变，通过带孔卷积对卷积核进行膨胀处理，使感受野同时增大，丰富卷积之后的图像信息。

输入分辨率为300×300的图片，3×3的卷积核，滑动步长(水平方向和垂直方向)为1，输出150×150×64，该输出通过对2×2的池化层进行池化操作获取。

匹配真实框与默认框，建立真实框与默认框的关系。设定两框之间的阈值比默认框高，促使产生若干个重叠的默认框，便于不再只针对最大重叠默认框进行选择，保证置信度的精准。

假设第i个默认框匹配p类别的第j个真实框，其中，大于默认框的阈值有若干个，位置损失加上置信损失为目标损失函数，即

(1)

其中，默认框数量为N，位置损失为Lloc(x，l，g)，置信损失为Lconf(x，c)，α为1，位置损失表示真实框与损失框之间的损失，即

(2)

置信度损失为

(3)

SSD算法不受单尺度检测的限制，对于边框类别分数和位置偏移的预测是通过小卷积预测器完成，可显著提升检测精度。同时，在单个网络中封锁住所有计算，确保在保证精度的基础上对整体实时性具有提升作用。

2.2 码垛机械臂关节速度纠偏PID控制

2.2.1 直流电机调速控制

以SSD算法识别结果为基础，采用PLC为控制器，针对码垛机械臂的电机转速和电流采用PWN调速方式进行PID控制[9]。调速模型如图2所示。

图2 直流电机动态PWM调速模型

对于外界干扰需要进行及时处理，不然会引起输出值变化产生误差，无刷直流电机调速模型能够通过PID控制实现一定程度的干扰调节，有效减少转速因干扰而产生的影响[10]。

2.2.2 PID纠偏实现方法

采用左轮和右轮转速偏差，用以表示码垛机械臂的运动过程，PID纠偏控制公式为

(4)

其中，u表示驱动电压控制电压，e(k)表示系统偏差，ec表示系统变化变化率，Kp表示比例系数，KI表示积分系数，KD表示微分系数。

由于当前状态决定本次输出值，在计算过程中，叠加系统偏差进行求和，导致工作量偏大，为了减少偏大的瞬间微分值，通过监看测定值进行PID指令控制，如图3所示。

图3 PID指令控制框图

PID表示公式为：

(5)

其中，u(k)表示输出值，uPV(k)表示测定值，uSV(k)表示目标值。

在PID控制器中，输入SV偏差和PV偏差，分别设置比例系数参数、积分系数参数和微分系数参数，对直流电机进行调速。根据测定值PV的微分状况，测定值变化量与车轮速度变化量成正比，当测定值变化量与输出值成反比时，利用PWM波形式对电机进行调速。

3 码垛机械臂关节运动轨迹规划实现

码垛机械臂关节角利用运动学逆解的方式进行目标位姿与预定轨迹的求解，结合2.2小节的PID机械臂关节速度纠偏方式，共同完成码垛机械臂关节运动轨迹规划。

3.1 轨迹规划法

利用“5-3-5”轨迹规划法控制抓取放置过程中码垛机械臂的速度、加速度和精度。具体步骤为：①起点→抬升点，采用五次多项式进行规划；②抬升点→下降点，采用三次多项式进行规划；③下降点→终点，采用采用五次多项式进行规划。各个关节的目标轨迹多项式方程为

(6)

其中，θ1、θ2和θ3分别表示Ⅰ轴、Ⅱ轴和Ⅲ轴的关节变量，ζ表示轨迹。

完全满足以下四个条件才能体现四线运动连续性，即：

①轨迹中任何一段初始和末端两个位置需要与四个(A、B、C、D)关节角位置保持一致；

②轨迹第一段初始和第三段末端的角速度和角加速度相同，且为0；

③轨迹前后段之间角位置、角加速度和角加速度保持一致；

④根据关节轴的关节变量，设置轨迹ζ1f、ζ2f和ζ3f和所需时间，分别为2s、4s和2s。

3.2 轨迹规划动力学分析

码垛机械臂每个关节运行需要具有较大的力和力矩，保证关节速度和加速度满足预期位置精度。利用拉格朗日构建系统动力学方程，码垛机械臂关节速度纠偏运动力矩公式为

(7)

展开式(7)得

(8)

其中，Fii表示有效惯量，Fi表示重力，Fiji和Fijj分别表示关节i、j之间的耦合惯量和耦合力矩，表示在关节i上关节j处速度产生的向心力。

每个关节驱动力矩方程为

(9)

(10)

重力项为

(11)

其中，m2、m3分别表示大臂小臂质量。li表示相应臂的长度。

设质心在l的中心，将惯性项、科里奥利力和向心力项、重力项的方程带入式(9)中计算各关节驱动力矩方程，即

(12)

(13)

根据上述分析，完成了码垛机械臂关节速度纠偏运动规划。

4 实验分析

4.1 实验环境和参数设计

采用实物码垛作业进行速度纠偏运动规划实验，码垛机械臂为某大型公司工程样机实验平台，如图4所示。

图4 码垛机械臂实验对象

实验条件：抓取对象重量为35 kg，直线上升高度385 mm，下降高度185 mm，机械臂运行参数如表1所示。

表1 运动参数约束

在码垛机械臂的运行轨迹上设置磁条轨迹，并在运动轨迹上安放四个大小为200 mm×200 mm的白色标记作为监测点，保证白色记号与磁条各自中心相重合，根据实际运行轨迹在白色标记上留下的痕迹作为验证偏差情况。

4.2 结果分析

为验证本文方法的有效行，实验中设置不同偏差比例系数进行实验，以磁条中心为界，偏左侧为正值，偏右侧为负值，测试点与标准轨迹间的偏差如表2所示。

表2 偏差距离表

由表2可知，当偏差比例系数为4000时，本文方法能够将偏差控制在±5mm之内，根据多次实验过程中实际运行轨迹，基本与标准轨迹能够保持一致，证明本文方法在码垛机械臂关机速度纠偏运动中的控制效果较好，具有较好的应用效果和适用性。

针对Ⅰ轴和Ⅱ轴关节角、角速度和角加速度的轨迹曲线，通过四线运动连续性条件和MATLAB软件编程绘制，具体轨迹曲线如图5所示。

图5 Ⅰ轴和Ⅱ轴轨迹曲线

由图5可知，在运动控制过程中所有关节角、角速度和角加速度轨迹曲线均连续且光滑，可有效避免关节变向运动的冲击，确保码垛机械臂平稳运行，有效提升各关节运行学性能，提升码垛机械臂工作效率。

为了进一步验证所提方法的有效性，实验对比本文方法、文献[6]局部闭链码垛机器人运动学分析及轨迹规划方法和文献[7]基于三次样条曲线的码垛机器人平滑轨迹规划方法，对目标进行识别。在码垛机械臂运动轨迹前置放规格为600×300×300mm的50个纸箱和一个400×350×280mm的纸箱作为识别对象，以成功抓取400×350×280mm的纸箱为目标，目标识别速度对比如图6所示。