基于余弦定理-PID的煤矸石分拣机器人动态目标稳准抓取算法.pdf

第4 5 卷第1 2 期 2 0 2 0 年1 2 月 煤炭学报 J O U R N A L0 FC H I N AC O A LS O C I E T Y V 0 1 ．4 5 D e c ． N o ．1 2 2 0 2 0 移动阅读 王鹏，曹现刚，马宏伟，等．基于余弦定理一P I D 的煤矸石分拣机器人动态目标稳准抓取算法[ J ] ．煤炭学报， 2 0 2 0 ，4 5 1 2 4 2 4 0 4 2 4 7 ． W A N GP e n g ，C A 0X i a n g a n g ，M AH o n g w e i ，e ta 1 ．D y n a m i ct a r g e ts t e a d ya n da c c u r a t eg r a s p i n ga l g o r i t h mo fg a n g u es o 卜 t i n gm b o tb a s e do nc o s i n et h e o r e m P I D [ J ] ．J o u m a lo fc h i n aC o a ls o c i e t y ，2 0 2 0 ，4 5 1 2 4 2 4 0 4 2 4 7 ． 基于余弦定理一P I D 的煤矸石分拣机器人动态目标 稳准抓取算法 王 鹏1 ’2 ，曹现刚1 ’2 ，马宏伟1 ’2 ，吴旭东1 ’2 ，夏 晶1 ’2 1 ．西安科技大学机械工程学院，陕西西安7 1 0 0 5 4 ；2 ．陕西省矿山机电装备智能监测重点实验室，陕西西安7 1 0 0 5 4 摘要针对煤矿生产中煤矸石分拣问题，提出机器人分拣方案，并对机器人在跟踪和分拣煤矸石 时出现延时、抖动、冲击、分拣速度低等现象进行了分析和讨论。通过分析发现，出现这些现象的主 要原因是煤矸石属于大质量、快速移动分拣对象，同时决定了机器人机械本体具有较大的惯性负 载，传统的跟踪算法由于较大的数据量不适于机器人对大质量、动态目标的快速跟踪与稳定抓取。 针对该问题，结合实际工况以及不同追踪方法的优点，提出了基于余弦定理一P I D 的动态目标抓取 算法。首先，利用梯形速度运动规律进行了轨迹规划，并建立了带有圆弧过渡的门字型轨迹；然后， 对动态环境下的大质量、动态目标抓取问题建立了数学模型，并针对该数学模型提出了基于余弦定 理一P I D 的动态目标抓取算法。该算法首先通过余弦定理计算出机器人理论抓取点，控制机器人快 速到达理论抓取点后，再通过位置一速度双环P I D 算法控制机械臂末端与煤矸石进行同步。算法 测试结果表明该算法通过余弦定理计算出理论抓取点，能在很大程度上减小机械臂跟踪煤矸石时 间，并使得机器人末端快速逼近目标，提高跟踪效率，并通过位置一速度双环P I D 算法控制机械臂 末端与煤矸石进行同步，以保证机械手对煤矸石进行稳准抓取，有效解决了机器人在抓取过程中出 现的延迟、抖动和冲击问题。样机实验结果表明利用该算法能使煤矸石分拣机器人以最优时间进 行大质量、动态目标的稳准抓取，单机器人抓取速度可达2 5 次／m i n ，验证了算法的准确性、高效性 与稳定性。 关键词煤矸石分拣；动态抓取；余弦定理；P I D 追踪；轨迹规划 中图分类号T D 6 7 ；T P 2 7 3文献标志码A文章编号0 2 5 3 9 9 9 3 2 0 2 0 1 2 4 2 4 0 0 8 D y n a m i ct a r g e ts t e a d ya n da c c u r a t eg r a s p i n ga l g o r i t l l I I lo fg a n g u es o r t i n g r o b o tb a s e do nc o s i n et h e o r e m ．P I D W A N GP e n 9 1 一，C A 0X i a n g a n 9 1 ”，M AH o n g w e i l ”，W UX u d o n 9 1 ”，X I AJ i n 9 1 2 1 ．S c o D fo ，肘e c 彻．i c 口2E 昭i 聊e “n g ，剧’Ⅱnu n i 钾坶妙D ，s c i e n c e 口砌‰ 加如∥，皿’Ⅱn7 1 0 0 5 4 ，c i M ；2 ．S ∞础i 匆儿6 D m ￡o ，y 铲 以肥肘e 以帆i ∞f 8 n dE 蛔r o m 托h n 池lE q u 啼瞅ml 眦m g e 呲M o n 主乇o r i n g ．∥ⅡR1 1 0 0 5 4 ．C h t M 、 A b s t r a c t T os o l V et h ep r o b l e mo fs o r t i n gc o a lg a n g u ei nc o a lm i n ep r o d u c t i o n ，ar o b o ts o r t i n gs c h e m ew a sp r o p o s e di n t h i sp a p e r ，a n dt h ep h e n o m e n ao ft i m ed e l a y ，j i t t e r ，s h o c ka n dl o ws o n i n gs p e e dw h e nt h er o b o ti st r a c k i n ga n ds o n i n g c o a lg a n g u ew e r ea n a l y z e da n dd i s c u s s e d ．I tw a sf b u n dt h r o u g ha n a l y s i st h a tt h em a i nr e a s o nf b rt h e s ep h e n o m e n ai s b e c a u s ec o a lg a n g u ei sal a 疆e s c a l e ，f a s t - m o V i n gs o r t i n go b j e c t ，a n di td e t e m i n e st h a tt h em e c h a n i c a lb o d yo ft h er o b o t 收稿日期2 0 1 9 一1 1 1 3修回日期2 0 2 0 一0 2 1 9责任编辑郭晓炜D o I l O ．1 3 2 2 5 ／j ．c nk i j c c s ．2 0 1 9 ．1 5 6 5 基金项目国家自然科学基金面上资助项目 5 1 9 7 5 4 6 8 ；陕西省教育厅科学研究计划资助项目 1 8 J C 0 2 2 作者简介王鹏 1 9 8 6 一 ，男，陕西西安人，工程师，博士研究生。E m a i l k e l a n 4 7 1 2 6 ．c o m 万方数据 第1 2 期王鹏等基于余弦定理一P I D 的煤矸石分拣机器人动态目标稳准抓取算法 h a sal a r g ei n e r t i a1 0 a d ．T h et r a d i t i o n a lt r a c k i n ga l g o r i t h mi sn o ts u i t a b l ef o rf a s tt r a c k i n ga n ds t a b l eg r a b b i n go fl a r g e m a s sa n dd y n a m i ct a 唱e tb e c a u s eo ft h el a r g ea m o u n to fd a t a ．S p e c i f i ct ot h i sp m b l e m ，ad y n a m i ct a 唱e tg r a b b i n ga l g o r i t h mb a s e do nc o s i n el a w P I Dw a sp m p o s e di nc o m b i n a t i o no fa c t u a lw o r k i n gc o n d i t i o n sa n dt h ea d V a n t a g e so fd i f f b r e n tt r a c k i n gm e t h o d s ．F i r s to fa l l ，t h et r a j e c t o r yp l a n n i n gw a sc 枷e do u tb yu s i n gt h el a wo ft r a p e z o i d a lV e l o c i t ym o t i o n ，a n dt h eg a t e s h a p e dt r a j e c t o r 丫w i t ha r ct r a n s i t i o nw a se s t a b l i s h e d ．T h e nam a t h e m a t i c a lm o d e lw a se s t a b l i s h e df b r t h eg m b b i n go fl a 唱e m a s s ，d y n a m i ct a r g e t si nad y n a m i ce n v i r o n m e n t ，s p e c i f i ct ow h i c h ，ad y n a m i ct a r g e tg r a b b i n ga l g o r i t h mb a s e do nc o s i n el a w P I Dw a sp u tf o r w a r d ．7 r h i sa l g o r i t h mf i r s tc a l c u l a t e st h et h e o r e t i c a lg r a b b i n gp o i n to ft h e r o b o tt h m u g ht h el a wo fc o s i n e ，a n da f t e rc o n t m l l i n gt h er o b o tt or e a c ht h et h e o r e t i c a lg r a b b i n gp o i n tr a p i d l y ，i tc o n t r o l s t h ee n do ft h er o b o ta 瑚t os y n c h m n i z ew i t ht h ec o a lg a n g u et h r o u g ht h ep o s i t i o n s p e e dd u a l l o o pP I Da l g o r i t h m ．T h e t e s tr e s u l t so ft h ea l g o r i t h ms h o wt h a tt h ec a l c u l a t i o no ft h et h e o r e t i c a lg r a b b i n gp o i n tt h r o u g ht h el a wo fc o s i n ec a nr e d u c et h et i m et h a tt h er o b o ta H I lt r a c k st h ec o a lg a n g u et oag r e a te x t e n t ，a n dm a k et h er o b o te n dq u i c k l y 印p r o a c ht h e t a r g e t ，i m p r o v i n gt h et r a c k i n ge m c i e n c y ．T h ea l g o r i t h mc o n t r o l st h e r o b o ta 珊t os y n c h r o n i z ew i t ht h ec o a lg a n g u e t h r o u g ht h ep o s i t i o n - s p e e dd u a l l o o pP I Da l g o r i t h mi no r d e rt oe n s u r et h a tt h em a n i p u l a t o rg r a b st h ec o a lg a n g u es t a b l y a n da c c u r a t e l y ，w h i c he f k c t i v e l ys 0 1 v e st h ep I ．0 b l e mo ft i m ed e l a y ，j i t t e ra n ds h o c kd u r i n gt h eg r a b b i n g ．T h ee x p e r i m e n - t a lr e s u l t so ft h ep r o t o t y p es h o wt h a tt h ea l g o r i t h mc a ne n a b l et h ec o a lg a n g u es o n i n gr o b o tt og r a bt h el a r g e - m a s sd y - n a m i ct a 曙e t ss t a b l ya n da c c u m t e l ya tt h eo p t i m a lt i m e ，a n dt h eg r a b b i n gs p e e do fas i n d er o b o tc a nr e a c h2 5t i m e s ／ m i n ，w h i c hV e r i 6 e st h ea c c u r a c y ，e m c i e n c ya n ds t a b i l i t yo ft h ea l g o r i t h m ． K e yw o r d s c o a lg a n g u es o r t i n g ；d y n a m i cf e t c h i n g ；t h el a wo fc o s i n e s ；P I Dt r a c k i n g ；t r a j e c t o r yp l a n n i n g 在煤矿生产中，从原煤中分拣煤矸石是煤炭洗选 的首要环节，原煤人选率是评价煤矿生产效益的重要 指标⋯。矸石是影响煤矿煤炭生产质量的重要因 素旧J ，拣矸率则是评价煤矿分选效果的重要参数。 传统的干选旧’4 1 和湿选∞。1 方法在一定程度上提高了 煤炭入选率，但是其所带来的资源浪费、环境污染和 成本高等问题已不能满足绿色开采要求旧J 。特别是 大粒度 1 0 0 3 0 0m m 的矸石依然依靠人工分拣来 完成，存在人员需求多、拣矸效率低、劳动强度大、工 作环境劣等问题一J 。随着机器人、煤矸石识别H 叫等 技术的不断发展和成熟，利用机器人进行煤矸石分拣 已经成为一种新的研究方向，国内相关研究工作者在 该方向做了大量的研究工作，也取得了一定的研究成 果【l H2 | 。特别是在煤矸石视觉识别的基础上，多机 械臂协同分拣煤矸石技术的提出卜HJ ，打开了机器 人迈向煤矿煤矸石分拣领域的大门，也为这一研究方 向提供了新的思路。 煤矿生产中，煤和矸石通过带式输送机进行运 输，其运行速度高，分布随机，并且矸石质量和体积较 大，和传统工业领域传送带分拣相比，由于分拣对象 的特殊性，对机器人在负载、速度等参数的要求较高。 在分拣领域最核心的问题便是动态目标快速跟踪问 题，在这方面，大量研究人员在高速分拣领域做了充 分的实验研究，并且取得了显著的成果。目前常用的 方法主要有P T P P o i n t T o P o i n t ‘15 I 、比例导引 法H6 | 、金字塔寻优ⅢJ 、P I D 跟踪引等。这些方法通 过视觉获取目标实时位置，然后通过算法控制机器人 末端对目标进行动态抓取，具有较高的响应，分拣速 度可高达1 1 0 渺m i n l l9 I 。由于在工业高速分拣中， 其分拣对象属于小体积、小质量目标，在对机器人末 端控制时，只需要机器人末端与目标在位置上相遇便 可完成分拣；同时，分拣机器人为轻量化设计，机械系 统本身喷量负载小，这2 个因素决定了其具有较高的 响应速度和分拣速度。 但是对于煤矸石分拣来讲，由于分拣粒度在 1 0 0 3 0 0m m ，分拣质量在5 ～2 0k g ，这就决定了分 拣机器人机械本体系统具有较大陨量负载，也在一定 程度上影响了系统的响应。在前期试验中，利用以上 算法对煤矸石进行动态抓取实验时发现，机器人在动 态跟踪过程中出现动作延迟和抖动，并且在跟踪到目 标后进行抓取时有较大的冲击载荷，甚至对机械手造 成损坏。分析其原因主要有以下几个方面①由于 煤矸石分拣机器人机械本体惯量负载较大，同时算法 较为复杂，导致系统响应时间过长，进而出现机械臂 在运动过程中出现抖动现象；②由于煤矸石质量过 大，并且其运动速度较高，在对其进行抓取时若只采 用拦截的方式进行位置匹配，而不考虑其速度和加速 度的匹配，则会因为煤矸石具有较大的惯性力而出现 抓取时较大的冲击；③以上算法在控制机器人末端 逼近目标时虽然有较高的位置精度，但是应用在煤矸 石分拣机器人上时会出现跟踪时间过长而出现目标 超出机器人工作空间现象，最终导致抓取失败。再 万方数据 煤炭 学报 2 0 2 0 年第4 5 卷 柠，不旧的煤矿其含矸量不同，以含矸量1 5 ％的煤矿 为例，通过计算，要求分拣机器人分拣速度要达到3 0 次／m i n 以卜．，对机器人性能提出了很高的要求。因 此，研究适用于煤矿分拣机器人的动态目标快速、准 确、稳定的抓取算法对于提高煤矸石分拣机器人分拣 技术水平具有重要意义。 综合以E 分析，笔者结合以上算法的优点，提出 r 基于余弦定理一P I D 的机器人动态目标抓取算法。 该方法通过余弦定理计算出煤矸石理论抓取点，然后 控制机器人末端到达理论抓取点后转为P I D 跟踪， 最后使得机器人末端与煤矸石同步，有效解决了其他 算法冈迭代次数过多而产生的机械臂响应时问长、运 行抖动问题，同时确保机器人以最优的时间实现对煤 矸石同步跟踪，实现机器人对大质量、动态煤矸石的 快速、准确和稳定抓取。 1 煤矸石分拣机器人原理 煤矸石分拣机器人如罔1 所示。多机械臂煤矸 彳i 分拣机器人系统由煤矸石识别系统、机械臂、E 位 机、机器人控制器、视觉伺服系统等组成。该系统通 过煤矸石识别系统对矸石进行识别并获取其位置和 姿态。然后将所获取的矸行信息进行数据打包发送 给匕位机。上位机在获取煤矸石信息后，根据煤矸石 位置进行排序工作，并通过多目标任务分配策略将抓 取任务下达给相应机械臂控制器。机械臂根据获得 的任务对目标进行监测，当日标进入机械臂工作空间 后机器人控制器对机械臂进行轨迹规划完成煤矸石 分拣。 0 机 机器人 控制器 “’速检捌0 装置 I 冬j 1 多机械臂煤矸彳形’拣机器人系统原理 - 1 i g ．1 S ’u c n l r eo fn l u l t i n l a l l i p u l a 【o rc a la n 1g a n g u e s o l l i l l gr o h J ts y s I e l l lb a s e 1o nn l a c h ；n ev i s i ，n 在L 述过程中，矸石经过视觉传感器采集信息之 后，随传送带向前运动，因此需要对工件进行动态追 踪抓取。传统的高速分拣机器人中所使用的跟踪方 法虽然呵以快速对运动目标进行拦截式抓取，而对于 煤矸石分拣来讲，由于煤矸石存在质量大、运动速度 快的特点，这种方法在应用过程中会产生较大的冲击 和振动。凶此，为了避免这种现象发生，要使机器人 术端与煤矸石快速相遇并同步。首先通过余弦定理 获得最小时问下机器人与煤矸石理论抓取点，并在机 器人末端到达理论抓取点后转为位置一速度双环P I D 控制，使得机器人末端和煤矸石在位置、速度和加速 度上同步，然后进行稳定抓取。 2 煤矿分拣机器人轨迹规划 在实际生产中，人工分拣区域的带式输送机为平 皮带，并且通过张紧装置，确保了皮带在运输过程中 始终保持水平，不会出现下坠现象，这也为机器人分 拣提供了很好的分拣条件，即机器人抓取高度相同。 机器人在进行分拣任务时一般分为“上升一追踪～ 下降一抓取”4 个环节，而通常采用的轨迹为门子型 轨迹，如图2 中P ．P ，P 6P 8 的轨迹，其中，S ，～S ，为机 器人轨迹长度。、而P ，P ，凡P 。轨迹中上升和追踪初始 阶段为直角过渡，这种过渡方式很容易造成振动和冲 ．打，对于机器人的高速运行造成严重影响。冈此，在 对机器人轨迹进行设计时，将直角过渡区域进行圆弧 过渡处理，如P ，P 1 尸。P ，P 7 心。 P j 三iI l 冬l2 点对点| ’] 宁J 髟彩L 迹 F i g ．2I ’ i n l l 卜p i n lg a l eg l y p hl l ’a j e c “ I 、f 根据机器人特性，同时保证机器人在运动过程t { J 运动性能的稳定、连续和不突变，采用梯形速度轨迹 规划，机器人加速度为n ，由加速度公式可得速度和 位移表达式。 速度” ￡ 表达式为 ，Ⅱf 匀加速阶段 ” ￡ J ”。。、 匀速阶段 1 【一o ￡ 匀减速阶段 式中，秽。。、为机械臂末端最大速度；o 为机械臂末端加 速度；￡为时间。 位移s ￡ 表达式为 士。f 2 匀加速阶段 ‰、￡ 匀速阶段 2 一士Ⅱ￡2 匀减速阶段 万方数据 第1 2 期王鹏等基于余弦定理一P I D 的煤矸石分拣机器人动态目标稳准抓取算法 在门字型轨迹中，J s 。，5 ，S 。，s ，为已知，可根据实 际工况设置机器人末端高度和圆弧过渡半径，各段的 运行时间可根据各段位移和边界条件求出。而J s 。则 由煤矸石抓取位置和机器人末端位置关系决定。 3 动态目标稳准抓取算法 为了提高煤矸石分拣机器人分拣效率，避免过长 的跟踪时间，首先通过余弦定理计算出机器人理论抓 取点，使得机器人末端与煤矸石快速逼近，然后通过 P I D 算法控制机器人末端与煤矸石同步。具体控制 v B 目标初始位置l 余弦定 露蠢基蒹 初始位置I 理论抓 ] 取点 流程如图3 所示。 为使机器人完成动态目标快速跟踪，系统采用分 层控制的思想，将机器人的轨迹跟踪控制分为两层 轨迹跟踪控制器和速度P I D 控制器。在初始阶段， 引入余弦定理，计算出机器人抓取目标的理论抓取 点，由轨迹规划器控制机器人末端到达理论抓取点。 然后转入位置与速度双环P I D 跟踪阶段，图3 中P 。， 秽，为煤矸石实时位置与速度，通过编码器实时获取， P 为机器人理论抓取点坐标，只，秽i 为机器人末端位 置和速度输出。该算法具体计算过程如下。 P g P 裟H 船H 筹蒙 位置检测ll 位置检测Il 速度检测 图3 动态目标跟踪原理 F i 昏3 S c h e m a t i cd i a g r a mo fd y n a m i ct a r g e t t r a c k i n g 3 ．1 动态目标理论抓取点计算方法 要实现机器人对煤矸石的准确抓取，最重要的是 煤矸石的定位精度。系统采用基于位置的视觉控制， 根据计算机视觉系统提供的煤矸石位置等信息来计 算煤矸石的抓取位置旧0 ‘2 l J 。当监测到煤矸石进入机 器人工作空间，则将矸石坐标发送至机器人轨迹规划 器，由轨迹规划器计算煤矸石理论抓取点坐标，动态 目标理论抓取点计算示意如图4 所示。 厘 段 世 髑 暴 刿 接 佰 非 带式输送机运行方向 图4 理论抓取点计算示意 F i g ．4 S c h e m a t i cd i a g r a mo ft h e o r e t i c a lp o i n tg r a bc a l c u l a t i o n 在计算理论抓取点时，设传送带匀速运动，当机 器人完成上一煤矸石的分拣或处于初始状态，分拣机 器人末端位于放置点R ，此时分拣机器人分拣目标为 A 处的矸石，假设机器人在B 点便可与矸石位置重 合。A 点坐标和放置点R 坐标均已知，设A 点坐标为 A 并。，y 。 ，该坐标由煤矸石识别系统获取，R 点坐标 为R 茗，， ，， ，传送带速度为”。，则有L 。 △功。，△￡为 从点A 运动到点曰的时间，因此点曰坐标为 戈。 △细d ，y 。 ，A R 的距离￡A R 为 L R √ 戈。一戈， 2 y 。一y ， 2 3 机器人末端在B 点和煤矸石相遇，则煤矸石从A 点运动到B 点所花费的时间与机器人末端从R 点运 动到曰点所用的时间相等，并且通过煤矸石和机器 人末端初始位置可获得￡鲋尺。那么在△A 觥中通 过余弦定理可建立关于时间￡的关系式。 c 。。[ 4 B R 垒幺二垒 4 c o s [ A 8 R 竺_ - ] 等里 4 二L A B L R [ A B R a r c t a n 尘三曼 5 VV 戈r 一戈g ￡ 口 △纫d ，△t 丁I 疋 己 乃 死，其中E 为S l 段 位移时间，咒为| s 段位移时间，L 为s ，段位移时 问，L L ，咒 瓦。t ，疋，■，瓦可根据式 1 ， 2 求出。门字型轨迹投影到水平面上B R 的距离为 L 口R J s 6 J s J s 5 3 6 s ；，．s ；分别为s 和5 。在水平面上的投影，其值可 根据具体工况来设定，为已知量。通过设定s 。，s 数 值，令机器人末端运动到J s ，阶段时，机器人末端处于 匀速运动阶段，则 S ， L 秽胁。 7 其中，瓦为未知量；秽胁。为机器人末端最大速度。联 立式 3 ～ 7 可得关于L 的方程 降 器轨控器 机人迹制 万方数据 煤炭 学报 2 0 2 0 年第4 5 卷 八乃 ”j 一嘛。、 碍 2 [ ∑l 西一 s s “ 胁。、一￡{ R ∥。I c o spL 一2z ，。I L j l R ∑t c o s 曰 s 5 2h 。， ∑1 ‘ o 8 式中，臼为[ A B 尺角度；口．，为带式输送机带速。 ．厂 乃 为无匀速段下机器人动态跟踪煤矸石的数 学模型。对方程／、 L 进行求解，如果方程无解，则 选择放弃跟踪，若方程有正负解，则选择绝对值最小 的解。在计算出s ，段的时问丁，后，将其代人B 点坐 标中可获取机器人与煤矸石理论抓取点。 3 ．2基于位置一速度双环P I D 同步跟踪方法 机器人分拣煤矸石不同于工业应用的高速分拣 机器人，其分拣门标质量大、运动速度快，采用传统的 拦截方式对口标进行直接抓取会对机械手造成较大 的冲击和振动导致抓取失败，甚至造成机械手损坏。 因此，机器人末端在到达理论抓取点时，并不能直接 执行抓取动作，要保证机器人末端与煤矸石在位置和 速度上同步后，再进行抓取动作，这样才能确保机械 手在抓取时以最小的冲击将煤矸石抓取。P I D 控制 算法是工业控制应用方面主要技术之一，它以数学结 构简单、稳定性好、性能可靠、调制方便等优点在机器 人控制方面J 。泛应用2 2 j 。冈此，当机器人末端到达 理论抓取点后，转入位置一速度P I D 跟踪，实现同步 后稳定抓取。P I D 时域控制表达式为 嘶 瑙．㈠ 瓤扪汕 ，n 警] 9 式中，m ￡ 为P I D 控制输出；K ．、8 f 为比例控制项； K 。，为比例系数；fs f d ￡为积分控制项；丁。为积分 f 。J 【】 时间常数；7 1 。掣为微分控制项；r I 为微分时问常 数；s f 为输入值与实际值的差值。 系统通过视觉系统定位后，开始读取带式输送机 编码器获取煤矸石实时位置与速度，并发送至机器人 控制器，机器人控制根据煤矸石坐标变化实时调整机 械臂末端，逐步使机器人末端与煤矸石保持位置一 致2 3 。2 41 。机器人控制器通过P I D 算法不断获取机器 人末端与煤矸石之间的位置和速度差，并牛成机器人 控制量使机器人末端在位置和速度保持一致进行抓 取。动态目标抓取过程示意如图5 所示。 当机器人末端到达第l 阶段的理沦抓取点后，马 } 切入双环P I D 控制阶段，系统通过视觉实时获取 待抓取矸石坐标 戈；，y 。，z 。 和速度 州”博，”镏 ，同时 读取机器人末端坐标 戈，，y 。z ， 与速度 ，％，移二， 。 罔5 动态目标抓取不意 F 嘻5S c h m a t i ‘ 1 i a g r a n lo f 1 y n a m j 。t a 唱e I Ⅺp t u r e 则目标在3 个方向的位置误差与速度误差￡ ￡ ， s 、 ￡ ，s ￡ 表示为 r s 。 ‘ z s x 占、 ￡ y 。一y ， 1 0 【占 ￡ 。。一z ， r 占 ‘ 口一；一戈一r { s 。 f 口、。一∥。 1 1 【s ，。 ￡ 秽。一∥， 根据式 1 0 可得出机械臂在￡时刻的位置调整 量m ￡ 和速度调整量m ， ￡ 分别表示为 砒Ⅲ，，㈤ ≯础 ∞] 1 2 眠㈩碱h ”≯￡㈩帆警] 1 3 式中，雠，为速度比例控制系数；瓦为速度微分常数； E z 为速度输人值与实际值得差值。 对于带式输送机运输来讲，煤矸石在胶带f 二只有 沿运输方向拥有速度，即只有移。。，，方向和z 方向速 度可认为是0 。因此在对机器人控制时，其位置和速 度分量可以表示为 州归卟心 扣巾 n 警] 拍Ⅲ小㈩ 净瓦㈩ 玎警] 圳瑙㈦ 扣“丁 警】 ‰㈩喝卜 f ≯￡如 帆警] 州牡K 卜㈩ ≯E “卅瓦警] 咄Ⅲh 卜㈩ 净％㈩。掣] 万方数据 鹏等基于余弦定理一P I D 的煤矸石分拣机器人动态目标稳准抓取算法 式中，t ，为速度积分时间常数。 通过位置一速度双环P I D 控制实现机械臂末 端与煤矸石进行同步，在执行抓取动作时，两者保 持相对静止，呵以有效的防止由于煤矸石质量大 引起的惯性量以及由于速度不匹配而产生的冲 击，通过实验，验证了机械臂在抓取过程中的平稳 性。 4 煤矸石动态抓取算法实验验证 实验采用w i n t l 【，w s1 0 操作系统，用M a f l a b 将本 算法与比例导引法、金字塔寻优法进行测试对比，测 试条件根据机器人实际运行参数和带式输送机运行 速度，目标初始位置为 1 ，1 ，机器人末端初始位置 为 0 ，0 ．5 ，机器人最大速度为4n ，／s ，最大加速度为 1 0n ，／s ，带式输送机速度为0 ．7n ／s ，测试结果如图 6 所示。 从图6 可以看f “，本算法在时间．卜明显优于其他 算法，图中点l 为本算法抓取点，点2 为金字塔寻优 算法抓取点，点3 为比例导引法抓取点、在计算过程 中比例导引法迭代次数过多，算法运行时问最长，金 图6 小l 司算．法追踪效果仿真结果 F 培6 s i l l l L l I a t 油r e s u I f so “r a c I i 1 1 9e 艉 l so f I i i f f e l e l l I a l g o r i t h n l s 字塔寻优算法比比例导引法运算时间短，本算法的运 行时间最小。， 为了进一步验证本算法的高效性1 j 可j j 性，以实 验室双机械臂煤矸石分拣系统为基础，用以卜几种算 法分别在不同带速下进行跟踪抓取实验并记录实验 结果，同时观察实验过程中机器人是否发，卜冲击、振 动等现象。实验统计结果见表1 。 表1动态目标抓取算法及对比算法测试结果 T a b l el D y n a m i ct a r g e tc a p t u r ea 1 9 0 r i t h ma n dc o m p a r i s o no fa l g o r i t h mt e s tr e s u l t s 实验中运行参数与仿真参数相同。表l 中￡。，为 本算法抓取时间；f 。；为比例导引法的抓取时间；￡．为 金’’塔寻优的抓取时间。由表l 可得，在同等条件 下，比例导引算法和金字塔寻优算法同步时问都大于 本文算法，并且，比例导引与金字塔寻优算法在抓取 过程中多次出现冲击、抖动现象。 阁7 为煤矸石机器人实验样机。煤矸石分拣机 器人采用直角坐标结构，可以在空问拥有4 个自由 度，并配备了视觉传感器。用实验室样机进行实际抓 取实验，测试在不n d 传送带速度下算法的准确性与稳 定性。， 样机实验结果见表2 。实验中漏抓率 0 ．3 ％，实 验对比了基于金字塔寻优的分拣效果，结果表明本文 算法具有更高的准确性、稳定性和高效性。在实验室 条件下，尊机器人分拣速度最高可达2 5 次／m i n ，拣矸 阁7 煤矸孑i 分拣饥器人实验样机 F i g ．7E x p P r i l n e n t a lp l ‘ 儿 l y p P fc o a lg a n g L l Ps o r l i n gr o h t 率可达4 0 ％。。而本课题组所设计的双机械臂煤矸私 分拣机器人分拣速度最高可达5 0 次／m i n ，通过统汁， 最高拣矸率叮达6 3 ％。 万方数据 4 2 4 6 煤炭学报 2 0 2 0 年第4 5 卷 表2 样机实验结果 T a b l e2E x p e r i I I l 明t a lr e s l l l t so fp r o t o t y p e 5 结论 1 提出的算法通过余弦定理求解出机械臂预 抓取点，并结合P I D 算法控制机械臂末端与煤矸石 同步。2 者的结合在很大程度上减少了同步跟踪时 间，提高了分拣效率。 2 结合实际工况，通过仿真与实验的方法对提 出的算法进行验证，对比不同算法测试结果，本文所 提出的算法计算精准、速度快，满足实时性要求。 3 样机对比实验结果表明利用本文算法能使 煤矸石分拣机器人最高以2 5 次／m i n 的速度进行分 拣操作，并且在分拣过程中系统有较高的稳定性，使 得分拣速度和拣矸率最大化。在后期的研究中，将着 力于多机械臂协同分拣，以提高煤矸石分拣机器人拣 矸率。 参考文献 R e f e r e n c e s [ 2 ] [ 2 ] [ 4 ] [ 5 ] [ 6 ] [ 7 ] M AD a n ，D U A NH o n g r I l ，L I UJ i a n g f e “g ．T h er o l eo fg a n g u eo n t h em i t i g a t i o no fm i n i n g i n d u c e dh 舶捌d sa l