基于CODESYS的机器人控制系统设计.pdf

SOFTWARE2023软 件第 44 卷 第 8期2023 年Vol.44,No.8基金项目：山东省科技型中小企业创新能力提升工程项目（2022TSGC2191）作者简介：王成涛（1997），男，硕士研究生，研究方向：机械电子工程。通讯作者：张芃（1973），男，硕士研究生，讲师，研究方向：精密磨削。基于 CODESYS 的机器人控制系统设计王成涛1 张芃1 王斌2 张涵1 张辛铎1(1.山东建筑大学机电工程学院，山东济南 250101；2.胶州市市场监督管理局，山东青岛 266300）摘要：随着机器人技术的不断发展，机器人在工业自动化、医疗卫生和服务业等领域得到广泛应用。为满足机器人控制系统对开放性、通用性和扩展性的要求，本文在 IEC61131-3 标准的基础上，使用 CODESYS 软件开发环境设计了一种机器人开放式运动控制系统，该系统能够实现机器人的正逆运动学求解、插补运动等功能，并设计了可视化界面进行操作。此外，还使用 UG MCD 模块进行了 UR5 机器人控制仿真，通过观察仿真过程中机器人的运行状态，验证了机器人控制系统的有效性。实验结果表明，该机器人控制系统运行稳定，具有良好的应用前景和广泛的适用性。关键词：机器人；CODESYS；运动学；控制系统中图分类号：TP242 文献标识码：A DOI：10.3969/j.issn.1003-6970.2023.08.015本文著录格式：王成涛,张芃,王斌,等.基于CODESYS的机器人控制系统设计J.软件,2023,44(08):065-068+084 Design of Robot Control System Based on CODESYSWANG Chengtao1,ZHANG Peng1,WANG Bin2,ZHANG Han1,ZHANG Xinduo1(1.School of Mechanical and Electric Engineering,Shandong Jianzhu University,Jinan Shandong 250101;2.Jiaozhou Market Supervision Administration Bureau,Qingdao Shandong 266300)【Abstract】：With the continuous development of robot technology,robots are widely used in industrial automation,health care and service industries.In order to meet the requirements of openness,versatility and expansibility of robot control system,this paper designs a robot open motion control system based on IEC61131-3 standard and CODESYS software development environment.The system can realize the functions of forward and inverse kinematics solution,interpolation motion and so on,and designs a visual interface to operate.In addition,the UG MCD module is used to simulate the UR5 robot control.By observing the running state of the robot in the simulation process,the effectiveness of the robot control system is verified.The experimental results show that the robot control system runs stably and has good application prospects and wide applicability.【Key words】：robot;CODESYS;kinematics;control system基金项目论文0 引言随着机器人技术的不断进步和广泛应用，机器人运动控制技术也面临着更高的要求1-3。为了满足不同加工工艺的需求、降低生产成本、缩短研发周期以及扩大应用范围，机器人控制系统需要更好的开放性、通用性和扩展性。然而，目前机器人控制系统通常采用专用的控制器，且缺乏统一的编程语言，这种封闭式的控制系统在应用时会受到各种软硬件平台的限制，影响了机器人控制系统的研发和使用效率。因此，为了满足现代化的需求，需要开发更加开放、灵活、可扩展的机器人控制系统，并保证机器人运动控制的精度和效率，促进机器人技术的进一步发展和应用4,5。在相关文献中，有研究者使用基于 IEC61131-3 标准的 PLCopen 轴组功能块实现了多组协调运动控制，但未进行可视化界面设计，也有研究者使用 CODESYS 自带的功能块开发了一款机器人运动控制器，该控制器仅适用于六自由度工业机器人，并不适用于协作机器人6,7。针对上述问题，本文以 IEC61131-3 标准为基础8，运用结构化文本 ST 语言，开发了六轴机器人通用型控制系统，包括正逆运动学求解和插补运动程序，并将66软 件第 44 卷 第 8 期SOFTWARE运动学正解逆解程序封装成符合 PLCopen 标准的功能块，同时开发了简单人机交互界面。1 运动控制系统结构设计本文采用德国 3S 公司开发的可编程控制系统开发平台 CODESYS V3.5 SP19 作为开发环境，使用 CODESYS软件自带的 ST 语言作为主要编程语言进行编程设计，运动控制程序、机器人的正逆运动学求解和插补算法程序均采用 ST 语言进行统一编程，其中关节私服的运动控制调用符合 PLCopen 标准的使能 MC_Power 和位置控制 SMC_ControlAxisByPos 功能块完成。人机交互界面用于输入程序，并实时显示程序内部变量的数值，通过观察这些变量的变化，可以直观地了解程序的运动控制效果，变量通讯用于系统与机器人之间的通讯，控制系统软件框架如图 1 所示。按钮和界面的出发和更新程序人机界面正逆解、插补运动程序变量通讯ST关节私服运动控制程序图 1 控制系统软件框架Fig.1 Software framework of control system2 机器人控制系统功能开发2.1 机器人运动学功能块设计CODESYS 软件本身的 Robotics 库自带有六轴机器人的正解逆解功能块，但是六轴机器人的 DH 参数模型被限制，仅适用于常见的六自由度工业机器人，其DH 参数只有 d1、d3、d4、d5、a1、a2、a3 可以设置，并且第四轴的方向确定，与协作机器人第四轴的位姿并不相同。因此，本文开发了通用正解逆解函数程序，并封装成功能块。机器人运动学正解功能块的作用是通过机器人每个关节的关节角度来确定末端执行器的位置和方向，运动学正解程序的流程图如图 2 所示。运动学正解程序中专门设计了正解所用的结构体和运算函数。第一步输出机器人每个关节的关节角度，把关节 1 的角度赋值给 DH 参数结构体数组 DHable.ItemArr1的关节角Thet，六个关节角依次类推，第二步DH参数转齐次矩阵函数HTF_MatrixSTD会把输入的关节角转换成齐次变换矩阵，第三步通过矩阵右乘函数 Matrix44PointMul 把第二步所有的齐次矩阵以右乘的方式相乘，得到运动学正解的结果 06T。本文在此基础上增加了一步，第四步通过矩阵转位置信息函数RotationMatrix2pos 得到齐次矩阵所包含的各关节的位置信息，封装后的运动学正解功能块如图 3 所示。输入引脚包含每个关节的关节角度输入和功能块执行变量，输出引 脚 DirectKinematics_1.6 到 DirectKinematics_6 是每个关节相对于基坐标系的位置信息。Directkinematics_1Directkinematics_2DirectKinematics_3DirectKinematics_4DirectKinematics_5Directkinematics_6EnableJ1J2J3J4J5J6Kinematics_1Kinematics图 3 运动学正解功能块Fig.3 Functional block of kinematics forward solution机器人运动学逆解功能块的作用是通过末端执行器的位置和方向来确定机器人每个关节的关节角度，与运动学正解功能块的作用正好相反，但更加复杂，求解时存在奇异点的问题，封装后运动学逆解功能块如图 4 所示。其中输入引脚除了目标点的位置和方向外，还有功能块执行变量 enable、逆解奇异点决策要求 SortType和插补角度间距允许值 IptAgMax，逆解奇异点决策要求 SortType 等于 1 时要求关节最大移动量不大于图 2 运动学正解功能块流程图Fig.2 Flow chart of functional block of kinematics forward solution图 4 运动学逆解功能块Fig.4 Function block of inverse kinematics solutionDHTable.ItemArri.Thet赋值HTF_MatrixSTD(GVL.DHTable.ItemArri)矩阵转换Matrix44PointMul(T_Arri,T_Htf_Arri+1)获得映射矩阵启动结束RotationMatrix2pos(T_Arri)获取关节信息lnverseKinematics_1InverseKinematics_2lnverseKinematics_3InverseKinematics_4lnverseKinematics_5lnverseKinematics_6lsNullEnablePxPyPzRxRyrzSortTypelptAgMaxlnverse_Kinematicslnverse_Kinematics_267王成涛 张芃 王斌等：基于 CODESYS 的机器人控制系统设计IptAgMax 设置值，等于 0 时要求总关节移动量最少，以保证在不同的运动插补逆解时，不会出现奇异点；输出引脚除了六个关节的关节角度还包括无解输出变量，当目标位置无解时，IsNull 置 TRUE。2.2 插补功能设计直线插补功能块的作用是根据给定的起点和终点，计算机器人末端执行器在运动过程中的位置、速度和加速度等信息，实现机器人末端沿着一条直线路径移动。直线插补运动控制功能流程图如图 5 所示。Inverse_Kinematics(SortType:=0)逆解，检查是否有解按固定时长LR_MovingTime:=5，机器人末端先到达起始点开始计算当前坐标到目标坐标的距离LR_MoveLen时长LR_MovingTime起始点tmp1Ps和目标点tmp2Ps的坐标赋值结束设置加速、匀速和减速三个阶段的参数Kinematics正解更新各关节的坐标Inverse_Kinematics(SortType:=0)逆解各关节角度Inverse_Kinematics(SortType:=0)逆解，检查是否有解YN图 5 直线插补运动控制功能流程图Fig.5 Flow chart of linear interpolation motion control function首先是起始点和目标点的坐标赋值，调用 Inverse_Kinematics 逆解功能块逆解，SortType 设置为 0 要求关节移动量最少，判断是否有解，第二步，机器人末端先移动到起始点，再根据距离和时长规划好加速、匀速和减速三个阶段的参数，最后调用 Kinematics 正解检查更新，并把每个关节的位置信息显示到人机界面，如图 6 所示，末端轴的位置和 J6 的位置坐标跟所设置的目标点参数一致。阿基米德螺旋线插补运动的目标是用阿基米德螺旋线包住上半球，同时 J6 的 z 方向指向球心，其插补运动控制功能流程图与直线插补类似，如图 7 所示。3 仿真环境的搭建与实验验证将上述机器人运动学功能和插补运动功能等程序使用 ST 语言在 CODESYS 软件中编程实现。通过 OPC UA与 UG MCD 模块建立通讯，搭建好仿真平台。上位机利用 CODESYS 软件自带的驱动 CODESYS SoftMotion Win V364 版本 3.5.19.0；下位机通过 MCD 机电概念设计建立刚体，配置基本机电对象、运动副和位置控制信图 6 直线插补人机界面显示Fig.6 Display of linear interpolation human-machine interface图 7 阿基米德螺旋线插补运动控制功能流程图Fig.7 Flow chart of Archimedes helix interpolation motion control functionPoint2Point1LineBall目标点的参数目标点的参数目标球的参数txtytz-300-200100rxryrz18000txtytz100.00-200.00100.00rxryrz180.000.000.00R0.00cxcycz0.000.000.00末端轴的位姿xyzrxryrzpxpypz100.00-200.00100.00180.000.000.00J4J5J6J1J2J379.97-71.25139.9390.00-10.0321.320.00-23.79-48.63100.00100.0082.640.00-134.52-274.96-200.00-200.00-298.18162.50564.95199.60199.60100.00199.60机器人末端移动至球顶部起始点，调整姿态，坐标赋值开始按固定时长LR_MovingTime:=5,机器人末端先到达起始点球半径BallR，球心位置BallCx、BallCy和BallCz参数赋值设置3圈直径为目标直径，计算阿基米德螺旋线参数，计算机器人末端指向欧拉角Inverse_Kinematics(SortType:=1)逆解各关节角度结束Kinematics正解更新各关节的坐标Inverse_Kinematics(SortType:=0)逆解，检查是否有解NY68软 件第 44 卷 第 8 期SOFTWARE号，再通过外部信号设置搜索 opc.tcp:连接 CODESYS SoftMotion 驱动，在 CODESYS 软件中通过符号配置设置好要通讯的变量，通讯成功后 UG MCD 服务器信息状态栏显示相连，如图 8 所示。外部信号配置OPC DA OPC UASHM MATLAB PLCSIM AdvTCPUDPPROFINET FMU CMVM服务器信息端点URLopc.tcp:/LAPTOP-FD49DQJV:4840安全类型状态所有者部件None-None相连UR5_step时间设置订阅图 8 OPC UA 通讯状态Fig.8 OPC UA communication status以丹麦公司的经典机械人 UR5 为例，建立好通讯后，在人机界面输入好机器人模型参数，如图 9 所示。点击人机界面 Line 按钮，即可切换为直线插补功能，仿真结果如图 10、图 11 所示。J4J5J6J1J2J3daMaxAgMinAg MAgSpd000000133.399.799.6162.5000000-425-392.290-90%.2f9000270360360360360270-270-360-360-360-360-270150150150150150150图 9 机器人模型参数设置人机界面Fig.9 Human-machine interface of robot model parameter settingPoint2Point1LineBall目标点的参数目标点的参数目标球的参数txtytz-300-200100rxryrz18000txtytz100.00-200.00100.00rxryrz180.000.000.00R0.00cxcycz0.000.000.00末端轴的位姿xyzrxryrzpxpypz-570.24-228.4574.92180.000.000.00J4J5J6J1J2J39.30-30.7662.3390.00-80.7058.430.00-360.41-690.17-570.24-570.24-668.630.00-59.01-113.01-228.45-228.45-244.56162.50379.86174.52174.5274.92174.52 图 10 UR5 机器人直线仿真起始点Fig.10 Starting point of linear simulation of UR5 robot通过使用 CODESYS 中数据跟踪插件可以观察运动过程中机器人末端轴的相对于基坐标系的坐标变化以及各轴速度变化，如图 12、图 13 可以看出，在直线插补过程中，机器人末端轴的坐标变化曲线和机器人每个关节速度变化曲线连续平滑，没有突变，且机器人没有明显的抖动，可以平稳运行。4 结论本文根据机器人控制系统对开放性和扩展性的要求，在 CODESYS 软件开发环境下，基于 IEC61131-3标准设计了一种机器人的通用型运动控制系统，通过UG MCD 模块进行了 UR5 机器人联合仿真，实现了机器人的正逆运动学求解以及插补运动功能，验证了机器图 12 UR5 机器人直线仿真末端轴坐标变化曲线Fig.12 UR5 robot linear simulation end axis coordinate change curve图 13 UR5 机器人直线仿真各轴速度变化曲线Fig.13 UR5 robot linear simulation speed curve of each axisPoint2Point1LineBall目标点的参数目标点的参数txtytz-300-200100rxryrz18000txtytz100.00-200.00100.00rxryrz180.000.000.00R0.00cxcycz0.000.000.00末端轴的位姿xyzrxryrzpxpypz100.00-200.00100.00180.000.000.00J4J5J6J1J2J379.97-71.25139.9390.00-10.0321.320.00-23.79-48.63100.00100.0082.640.00-134.52-274.96-200.00-200.00-298.18162.50564.95199.60199.60100.00199.60图 11 UR5 机器人直线仿真目标点Fig.11 Target point of UR5 robot linear simulation30sX10040s50s1m1m10sYZ0300200-400-500-200-300-100-400-800-200-600400060020030s40s50s1mJ1J2J3J4J5J6 下转第84页84软 件第 44 卷 第 8 期SOFTWARE在船舶轨迹聚类中应用了基于密度的 DBSCAN 聚类算法。该算法仅需设置两个参数，不需要指定簇的个数，并且可以发现任意形状的簇，在检测任务中善于发现离群点。而 KANN-DBSCAN 的优势在于无需手动输入参数，从而在一定程度上减少了人为选择参数对聚类结果的影响，保证聚类结果的准确性与科学性，尽可能地降低用户操作的复杂程度。通过 Python 语言完成了船舶轨迹的聚类。在系统开发部分，基于前后端分离的思想，后端数据处理与聚类算法基于 Python 设计，根据任务的不同，通过引用NumPy、Flask、Sklearn 等 Python 库以实现数据清理、操作、建模、可视化等目标，并进行可视化展示。为验证算法的实现，本文选取了一部分渤海附近海域的船舶 AIS 真实数据，将轨迹数据导入该系统并进行了测试。结果显示，通过该系统可以得到良好的船舶 AIS 轨迹聚类效果，算法中的每一个步骤均正常实现。参考文献1 刘畅.船舶自动识别系统(AIS)关键技术研究D.大连:大连海事大学,2013.2 HUTTENLOCHER D P,KLANDERMAN G A,RUCKLIDGE W A.Comparing Images Using the Hausdorff DistanceJ.IEEE Transactions on Pattern Analysis and Machine Intelligence,1993.3 魏龙翔,何小海,滕奇志,等.结合Hausdorff距离和最长公共子序列的轨迹分类J.电子与信息学报,2013,35(4):784-790.4 ESTER M,KRIEGEL H P,SANDER J,et al.A Density-Based Algorithm for Discovering Clusters in Large Spatial Databases with NoiseJ.AAAI Press,1996.5 徐良坤,任律珍,周世波.船舶AIS轨迹聚类方法研究进展综述J.广州航海学院学报,2019,27(2):7-12+47.6 LIU B,SOUZA E N D,MATWIN S,et al.Knowledge-based Clustering of Ship Trajectories Using Density-based ApproachC/IEEE International Conference on Big Data.IEEE,2014.7 罗启福.基于云计算的DBSCAN算法研究D.武汉:武汉理工大学,2013.8 李东枫.基于AIS大数据的船舶危险会遇热点区域挖掘研究D.广州:华南理工大学,2017.9 KIM J H,CHOI J H,YOO K H,et al.AA-DBSCAN:An Approximate Adaptive DBSCAN for Finding Clusters with Varying DensitiesJ.The Journal of Supercomputing,2018.10 李宗林,罗可.DBSCAN算法中参数的自适应确定J.计算机工程与应用,2016,52(03):70-73+80.11 李文杰,闫世强,蒋莹,等.自适应确定DBSCAN算法参数的算法研究J.计算机工程与应用,2019,55(5):1-7+148.12 罗文华,许彩滇.利用改进DBSCAN聚类实现多步式网络入侵类别检测J.小型微型计算机系统,2020,41(8):1725-1731.人控制的有效性，所设计的机器人控制系统达到了设计的目的，具有重要的应用价值。参考文献1 BURKON A P,KRASILNIKYANTS E V.Design Principles for Motion Control System SoftwareJ.Automation and Remote Control,2014,75(5):935-944.2 KOZLOWSKI K.Special Issue:Robot Motion and ControlJ.Journal of Intelligent&Robotic Systems,2018,93(3/4):617-619.3 PAN L,GAO T,XU F,et al.Enhanced Robust Motion Tracking Control for 6 Degree-of-freedom Industrial Assembly Robot with DisturbanceJ.International Journal of Control,Automation and Systems,2018,16(2):921-928.4 陈梅,王舒润.基于PLCopen的电子凸轮功能块算法的研究J.控制工程,2020,27(1):121-126.5 徐胜华,徐建明,赵帅.基于PLCopen的六轴工业机器人运动控制功能块设计J.计算机测量与控制,2019,27(3):97-102.6 王夫康,张得礼,周伟.PLCopen轴组运动功能块的研究与实现J.机电工程,2016,33(12):1477-1482.7 王耀东,徐建明,徐胜华.基于CODESYS平台的六自由度工业机器人运动控制器设计J.计算机测量与控制,2018,26(9):103-107+125.8 李飞江,李翔龙.基于IEC61131-3标准运动控制器的设计与应用J.组合机床与自动化加工技术,2019(3):17-19+23.上接第68页