Track planning of coal gangue sorting robot for dynamic target stable grasping
-
摘要: 针对机器人分拣煤矸石时，因输送带打滑、左右摆动而造成矸石定位不准确、机械臂末端抓取失败和载荷冲击等问题，提出了一种基于机器视觉的煤矸石分拣机器人动态目标稳定抓取轨迹规划方法。首先，采用基于HU不变矩图像匹配算法对目标矸石进行匹配识别并获取目标矸石位姿；其次，分别建立机器人和相机−机器人运动学方程，并进行正逆求解，实现基于视觉的目标矸石精确定位；最后，采用位置−速度−加速度三环PID控制算法进行目标矸石动态跟踪，即位置环控制器的输入为获取的目标矸石精确位置，位置环控制器的输出作为速度环控制器的输入，速度环控制器的输出作为加速度环控制器的输入，将加速度环控制器的输出叠加到伺服电动机上，使机械臂末端与目标矸石达到位置、速度同步运动的效果，实现平稳快速抓取。采用Matlab对三环PID控制算法、三维比例导引算法和三维偏置比例导引算法进行仿真对比，结果表明：对动态目标的跟踪抓取在追随式、同步式和拦截式3种情况下，三环PID控制算法的响应时间、跟踪抓取时间均较比例导引算法及偏置比例导引算法短，且三环PID控制算法在整个过程中各轴速度、加速度连续、平滑，没有出现突变情况，可实现动态目标同步跟踪、精准抓取。在煤矸石分拣系统平台上应用三环PID控制算法、比例导引算法和偏置比例导引算法进行适应性实验，结果表明：3种算法在机器人运行时各个关节均未超限；三环PID控制算法完成抓取的平均时间比比例导引算法和偏置比例导引算法短；三环PID控制算法在抓取点的平均速度偏差在1 mm/s左右，跟踪速度偏差较小，可满足对高速度目标的同步跟踪、精准抓取要求。Abstract: When the robot is used to sort coal gangue, in order to solve the problems such as inaccurate positioning of gangue, failure of grasping by end of the manipulator and load impact caused by slippage and left-right swing of belt conveyor, a track planning method of coal gangue sorting robot for dynamic target stable grasping based on machine vision is proposed. Firstly, the target gangue is identified and the pose of the target gangue is obtained by using the HU moment invariants image matching algorithm. Secondly, the kinematic equations of the robot and the camera-robot are established respectively, and the forward and inverse solutions are carried out to realize the accurate positioning of the target gangue based on vision. Finally, the position-velocity-acceleration three-loop PID control algorithm is used to dynamically track the target gangue. The input of the position loop controller is the obtained precise position of the target gangue, the output of the position loop controller is used as the input of the velocity loop controller, the output of the velocity loop controller is used as the input of the acceleration loop controller, and the output of the acceleration loop controller is superimposed on the servo motor. Therefore, the end of the manipulator and the target gangue can achieve the effect of synchronous movement of position and velocity, so as to achieve stable and fast grasping. Matlab is used to compare the three-loop PID control algorithm, the three-dimensional proportional navigation algorithm and the three-dimensional biased proportional navigation algorithm. The results show that in the following, synchronous and intercepting cases of the tracking and grasping of dynamic targets, the response time and tracking and grasping time of the three-loop PID control algorithm are better than those of the proportional navigation algorithm and the biased proportional navigation algorithm. And the three-loop PID control algorithm is continuous and smooth in the speed and acceleration of each axis in the whole process without sudden change, which can realize synchronous tracking of dynamic targets and precise grasping. The three-loop PID control algorithm, proportional navigation algorithm and biased proportional navigation algorithm are applied to the coal gangue sorting system platform to carry out adaptability experiments. The results show that the three algorithms do not exceed the limit of each joint during robot operation. The average time of the three-loop PID control algorithm to complete the grasping is shorter than those of the proportional navigation algorithm and the biased proportional navigation algorithm. The average speed error of the three-loop PID control algorithm at the grasping point is about 1 mm/s, and the tracking speed error is small, which can meet the requirements of synchronous tracking and precise grasping of high-speed targets.
-
图 3 基于HU不变矩的动态目标匹配流程
Figure 3. Flow chart of dynamic target matching based on HU moment invariants
图 5 煤矸石分拣机器人运动学坐标系模型
Figure 5. Kinematics coordinate system model of coal gangue sorting robot
图 7 追随式3种算法动态目标轨迹规划曲线
Figure 7. Dynamic target track planning curves of three algorithms under following track planning mode
图 8 追随式3种算法X轴向位置、速度、加速度变化曲线
Figure 8. X axial position, velocity and acceleration curves of three algorithms under following track planning mode
图 9 追随式3种算法Y轴向位置、速度、加速度变化曲线
Figure 9. Y axial position, velocity and acceleration curves of three algorithms under following track planning mode
图 10 追随式3种算法Z轴向位置、速度、加速度变化曲线
Figure 10. Z axial position, velocity and acceleration curves of three algorithms under following track planning mode
图 11 同步式3种算法动态目标轨迹规划曲线
Figure 11. Dynamic target track planning curves of three algorithms under synchronous track planning mode
图 12 同步式3种算法X轴向位置、速度、加速度变化曲线
Figure 12. X axial position, velocity and acceleration curves of three algorithms under synchronous track planning mode
图 13 同步式3种算法Y轴向位置、速度、加速度变化曲线
Figure 13. Y axial position, velocity and acceleration curves of three algorithms under synchronous track planning mode
图 14 同步式3种算法Z轴向位置、速度、加速度变化曲线
Figure 14. Z axial position, velocity and acceleration curves of three algorithms under synchronous track planning mode
图 15 拦截式3种算法动态目标轨迹规划曲线
Figure 15. Dynamic target track planning curves of three algorithms under intercepting track planning mode
图 16 拦截式3种算法X轴向位置、速度、加速度变化曲线
Figure 16. X axial position, velocity and acceleration curves of three algorithms under intercepting track planning mode
图 17 拦截式3种算法Y轴向位置、速度、加速度变化曲线
Figure 17. Y axial position, velocity and acceleration curves of three algorithms under intercepting track planning mode
图 18 拦截式3种算法Z轴向位置、速度、加速度变化曲线
Figure 18. Z axial position, velocity and acceleration curves of three algorithms under intercepting track planning mode
表 1 煤矸石分拣机器人运动结构参数
Table 1. Motion structure parameters of coal gangue sorting robot
转换矩阵 运动结构参数 ${{\boldsymbol{M}}}_{1}^{{\rm{W}}}$ α1 l1 d1 θ1 ${{\boldsymbol{M}}}_{2}^{1}$ α2 l2 d2 θ2 ${{\boldsymbol{M}}}_{{\rm{E}}}^{2}$ α3 l3 d3 θ3 
下载: 导出CSV
表 2 3种算法运动仿真参数
Table 2. 3 kinds of algorithm motion simulation parameters
轨迹规划算法 初始速度
/(m·s−1)加速度
/(m·s−2)加速时间
/s偏置比例导引 0 10 0.3 比例导引 0 10 0.3 三环PID控制 0 − −
下载: 导出CSV
表 3 3种算法实验结果
Table 3. Experimental results of three algorithms
（∆X，∆Y，∆Z）/m
v
/(m·s−1)tPID
/stB
/stP
/sESPID
/mmESB
/mmESP
/mmEVPID
/(m·s−1)EVB
/(m·s−1)EVP
/(m·s−1)(0.5，0.5，0.4) 0.9 0.425 0.543 0.556 1.18 0.92 0.82 0.001 5 2.101 2.113 1.0 0.469 0.564 0.564 1.01 1.03 1.13 0.001 2 2.003 2.011 1.1 0.486 0.588 0.594 0.92 1.15 0.87 0.000 9 1.904 1.910 (0，0.5，0.4) 0.9 0.301 0.396 0.426 1.11 1.03 0.85 0.000 4 2.106 2.101 1.0 0.302 0.404 0.450 1.04 1.31 1.16 0.000 6 2.010 2.004 1.1 0.310 0.414 0.465 0.94 1.03 1.07 0.001 1 1.916 1.905 (−0.5，0.5，0.4) 0.9 0.303 0.372 0.427 1.21 0.90 1.19 0.001 3 2.115 2.102 1.0 0.308 0.369 0.456 1.06 1.03 1.10 0.000 9 2.013 2.009 1.1 0.311 0.367 0.492 1.04 0.92 0.97 0.001 4 1.907 1.913
下载: 导出CSV
-
[1] 尹建平. 平朔矿区煤矸石山生态修复模式[J]. 露天采矿技术,2021,36(4):87-88.YIN Jianping. Ecological restoration model of coal gangue dump in Pingshuo mining area[J]. Opencat Mining Technology,2021,36(4):87-88. [2] 董增澳,李萍,贾一雪,等. 煤矸石综合利用与资源化处理研究进展[J]. 环境保护前沿,2021,11(2):363-371. doi: 10.12677/AEP.2021.112040DONG Zeng'ao,LI Ping,JIA Yixue,et al. Research progress on comprehensive utilization and resourceful treatment of coal gangue[J]. Advances in Environmental Protection,2021,11(2):363-371. doi: 10.12677/AEP.2021.112040 [3] 李宁. 煤矸分拣机器人控制系统研究[D]. 西安: 西安科技大学, 2019.LI Ning. Study on the control system of coal mine sorting robots[D]. Xi'an: Xi'an University of Science and Technology, 2019. [4] 曹亦俊,刘敏,邢耀文,等. 煤矿井下选煤技术现状和展望[J]. 采矿与安全工程学报,2020,37(1):192-201.CAO Yijun,LIU Min,XING Yaowen,et al. Current situation and prospect of underground coal preparation technology[J]. Journal of Mining & Safety Engineering,2020,37(1):192-201. [5] 费佳浩. 煤矸分拣机器人结构设计及运动分析[D]. 西安: 西安科技大学, 2019.FEI Jiahao. Structure design and motion analysis of coal gangue sorting robot[D]. Xi'an: Xi'an University of Science and Technology, 2019. [6] CROFT E. A,FENTON R. G,BENHABIB B. Optimal rendezvous-point selection for robotic interception of moving objects[J]. IEEE Tranactions on Systems,Man,and Cybernetics,1998,28(2):192-204. [7] ZHANG Zhengyou. Camera calibration with one-dimensional objects[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence,2004,26(7):892-899. doi: 10.1109/TPAMI.2004.21 [8] 张朝阳,周惠兴,曹荣敏,等. 基于金字塔形寻优的传送带动态抓取研究[J]. 制造业自动化,2015,37(9):92-95. doi: 10.3969/j.issn.1009-0134.2015.09.026ZHANG Chaoyang,ZHOU Huixing,CAO Rongmin,et al. Research on dynamic conveyor grasping with pyramid search algorithm[J]. Manufacturing Automation,2015,37(9):92-95. doi: 10.3969/j.issn.1009-0134.2015.09.026 [9] LIN Yunhan,MIN Huasong,ZHOU Haotian,et al. A human-robot-environment interactive reasoning mechanism for object sorting robot[J]. IEEE Transactions on Cognitive & Developmental,2018,10(3):611-623. [10] 刘子龙. 基于机器视觉的快速分拣食品包装系统研究[D]. 杭州: 浙江工业大学, 2015.LIU Zilong. The research quick sorting food packaging system based on machine vision[D]. Hangzhou: Zhejiang University of Technology, 2015. [11] 罗绍涵,张禹,丁庆勇,等. 基于传送带实时调速的Delta机器人分拣方法[J]. 自动化应用,2021(2):80-83.LUO Shaohan,ZHANG Yu,DING Qingyong,et al. Delta robot sorting method based on real-time speed regulation of conveyor belt[J]. Automation Applications,2021(2):80-83. [12] 高涵,张明路,张小俊. 冗余机械臂空间轨迹规划综述[J]. 机械传动,2016,40(10):176-180.GAO Han,ZHANG Minglu,ZHANG Xiaojun. A review of the space trajectory planning of redundant manipulator[J]. Journal of Mechanical Transmission,2016,40(10):176-180. [13] 王铮,戴坚锋,钱振宇,等. 面向传送带作业系统的机器人目标跟踪与抓取策略研究[J]. 计算机测量与控制,2016,24(11):85-90.WANG Zheng,DAI Jianfeng,QIAN Zhenyu,et al. Conveyor belt operating system oriented robot target tracking and grasping strategy research[J]. Computer Measurement & Control,2016,24(11):85-90. [14] 曹现刚,李宁,王鹏,等. 基于比例导引法的机械臂拣矸过程轨迹规划方法研究[J]. 煤炭工程,2019,51(5):154-158.CAO Xiangang,LI Ning,WANG Peng,et al. Research and simulation on priority and path planing of manipulator gangue picking[J]. Coal Engineering,2019,51(5):154-158. [15] 王鹏,曹现刚,马宏伟,等. 基于余弦定理−PID的煤矸石分拣机器人动态目标稳准抓取算法[J]. 煤炭学报,2020,45(12):4240-4247.WANG Peng,CAO Xiangang,MA Hongwei,et al. Dynamic target steady and accurate grasping algorithm of gangue sorting robot based on cosine theorem -PID[J]. Journal of China Coal Society,2020,45(12):4240-4247. [16] 黄浩然. 基于HU不变矩的垃圾分类和识别[J]. 自动化应用,2020(8):74-76.HUANG Haoran. Garbage classification and recognition based on HU invariant moments[J]. Automation Application,2020(8):74-76. [17] HJOUJI A,EL-MEKKAOUI J,JOURHMANE M. Rotation scaling and translation invariants by a remediation of HU's invariant moments[J]. Multimedia Tools and Applications,2020,79(1):14225-14263. [18] 董昱,郭碧. 基于HU不变矩特征的铁路轨道识别检测算法[J]. 铁道学报,2018,40(10):64-70. doi: 10.3969/j.issn.1001-8360.2018.10.010DONG Yu,GUO Bi. Railway track detection algorithm based on HU invariant moment feature[J]. Journal of the China Railway Society,2018,40(10):64-70. doi: 10.3969/j.issn.1001-8360.2018.10.010 [19] 房国栋,高军伟,朱晨曦,等. 基于机器视觉的机械臂智能分拣系统[J]. 仪表技术与传感器,2020(12):72-76. doi: 10.3969/j.issn.1002-1841.2020.12.014FANG Guodong,GAO Junwei,ZHU Chenxi,et al. Intelligent sorting system for manipulator based on machine vision[J]. Instrument Technique and Sensor,2020(12):72-76. doi: 10.3969/j.issn.1002-1841.2020.12.014 [20] 王鹏,曹现刚,夏晶,等. 基于机器视觉的多机械臂煤矸石分拣机器人系统研究[J]. 工矿自动化,2019,45(9):47-53.WANG Peng,CAO Xiangang,XIA Jing,et al. Research on multi-manipulator coal and gangue sorting robot system based on machine vision[J]. Industry and Mine Automation,2019,45(9):47-53. [21] 周高鹏,徐驰. 基于PID控制的物流搬运机械自动化轨迹规划方法研究[J]. 自动化与仪器仪表,2020(7):54-57.ZHOU Gaopeng,XU Chi. Research on trajectory planning method of logistics handling machinery automation based on PID control[J]. Automation & Instrumentation,2020(7):54-57. [22] LIU Shuru,SHANG Zhanlei,LEI Junwei. Research on attack angle tracking of high speed vehicle based on PID and FLNN neural network[J]. International Journal of Intelligent Robotics and Applications,2019,3(4):383-391. doi: 10.1007/s41315-019-00112-4 [23] 王晓海,孟秀云,周峰,等. 基于偏置比例导引的落角约束滑模制导律[J]. 系统工程与电子技术,2021,43(5):1295-1302. doi: 10.12305/j.issn.1001-506X.2021.05.17WANG Xiaohai,MENG Xiuyun,ZHOU Feng,et al. Sliding mode guidance law with impact angle constraint based on bias proportional navigation[J]. Systems Engineering and Electronics,2021,43(5):1295-1302. doi: 10.12305/j.issn.1001-506X.2021.05.17 -
点击查看大图
计量
- 文章访问数: 320
- HTML全文浏览量: 64
- PDF下载量: 68
- 被引次数: 0
