路锥自动投收机械手轨迹规划及控制系统设计

黄 珍,项炎华,李 泽,吕能超

(1.武汉理工大学 自动化学院,武汉430070; 2.普宙飞行器科技(深圳)有限公司,武汉430074; 3.武汉长江通信智联技术有限公司,武汉430074; 4.武汉理工大学 国家水运安全工程技术研究中心,武汉430063)

国家自然科学基金资助项目(51678460，U1664262);国家科技支撑计划资助项目(2014BAGO1B0503)

黄 珍(1974—),女,教授,博士.E-mail:h-zhen@whut.edu.cn

路锥自动投收机械手轨迹规划及控制系统设计

黄 珍1,项炎华2,李 泽3,吕能超4

(1.武汉理工大学 自动化学院,武汉430070; 2.普宙飞行器科技(深圳)有限公司,武汉430074; 3.武汉长江通信智联技术有限公司,武汉430074; 4.武汉理工大学 国家水运安全工程技术研究中心,武汉430063)

以2自由度路锥自动投收机械手为研究对象,针对其路锥自动收放作业下的操作空间和关节空间的轨迹规划进行研究,并开展机械手控制系统的总体设计.结合实际工况控制需求,设计机械手结构并基于D-H参数法建立其运动模型.分析机械手轨迹需求,采用五次多项式函数插值法,对机械手运动轨迹进行规划,分别考虑机械手解耦控制和动态配合控制,对其末端运动轨迹的关键路径点进行设置.在Matlab环境中,以路锥自动投放为例,开展机械手轨迹规划仿真及分析,并对解耦控制和动态配合控制下机械手末端运动曲线进行对比分析.开展路锥自动投收机械手控制系统设计,并基于上述研究及仿真结果,实现对机械手运动过程控制.最后,基于实车测试平台及路锥自动投放试验,对机械手轨迹规划方法及实际工作效果进行验证.试验结果表明,提出的机械手运动轨迹规划方法及设计的控制系统,可以满足路锥自动收放的实际工作需求,取得较好效果.

路锥收放; 机械手; 轨迹规划; 仿真; 控制系统

目前我国道路交通事故居高不下,死亡人数居世界第一,道路交通安全形势严峻.2014年,我国道路交通事故196 812起,致死率为29.73%,其中高速公路交通事故致死率更是高达38.52%[1-2].交通事故应急救援不及时是导致致死率居高不下的主要原因之一,因此,对交通事故现场的快速、有效隔离成为重中之重.一方面可以防止二次事故的发生,降低损失,防止事态恶化,保护现场;另一方面可保障正常车辆安全通过事件路段,避免交通堵塞[3].目前事故现场交通隔离设施(主要为路锥)的布设主要采用人工方式,效率低,劳动强度大,危险性高.国内已研发出若干交通路锥自动、半自动收放车及相关投放、回收装置,如上海电控研究所于2012年研发的SY-100型交通路锥自动收放车,但目前还没有进入大规模应用阶段.现有相关路锥车大多数都是基于轻、中型货车作为车载平台,存在着体积庞大、结构复杂、机动性不强的问题.同时,国外相关产品亦存在上述问题,加上技术垄断、价格昂贵,因此,研究与设计更加实用、可靠、高效和智能化的交通路锥自动收放设备迫在眉睫[4].

机械手运动控制系统的研究成果给人类带来了极大的方便,因此,机械手控制的研究领域一直是人们关注的焦点[5-6].路锥自动收放装置中利用机械手结构在实现路锥收放的同时,可以很好地解决机械结构复杂的问题.对机械手结构进行最优轨迹规划,并为后续运动控制实现提供基础是至关重要的[7-9].

本文以武汉理工大学自主研发的路锥自动投收机械手为研究对象.首先,基于D-H参数法建立其运动模型;其次,在Matlab环境中,利用Robotic Tools工具箱对该机械手进行了建模、路径规划,并分别采用解耦和协调两种控制方案,进行了机械手运动控制仿真与对比分析,在仿真研究的基础上,开发了路锥自动投收机械手控制系统;最后,基于实车测试平台及路锥自动投放试验,对机械手轨迹规划方法及实际工作效果进行验证.

1 路锥自动投收机械手建模

为有效实现机械手自动投收任务,进行了理论仿真研究,得到机械手运动控制参数.基于该目标,简述路锥自动投收机械手组成结构和工作原理,并利用D-H参数法对被控对象——机械手进行建模.

1.1机械手组成

本路锥收放机械手主要针对高速公路交警使用的皮卡车和特种路锥进行设计,主要由滑轨机构、旋转臂机构及电磁铁组成[10-11],利用末端电磁铁的得、失电实现取锥和放锥.

1.2机械手自由度分析

以图1中电磁铁为参考点,机械手机构存在2种运动方式:① 沿滑轨的上下直线运动;② 绕旋转臂的旋转运动.图1中杆旋转臂末端与电磁铁的相对位置保持不变,故忽略该关节以简化模型.因此,机械手可看作为2自由度的运动机构.

图1 机械手组成结构Fig.1 The structure of manipulator

1.3机械手运动模型建立

为了确保所建立的模型与图1所示的机械手结构一致,设计了A,B,C和D4个关节点,其中仅A和C为运动关节.根据D-H建模方法建立该机械手的D-H坐标系,如图2所示,O点沿Z轴平移偏距d至关节A处,关节A点沿X轴平移L1至转动关节B,B点绕Z轴旋转θ角并将端点延长L2至C处,C点绕Z轴旋转90°并将端点延长L3至D处,D点即为机械手的末端.

各连杆参数与关节之间的关系如表1所示.其中L1为连杆AB的长度,L2为连杆BC的长度,L3为连杆CD的长度.即L1=18.7 cm,L2=61.2 cm,L3=30.0 cm.由于A，C为移动关节,则偏距d.关节角θ为关节变量.

图2 机械手关节D-H坐标系Fig.2 The D-H coordinate system of manipulator joint表1 机械手关节和连杆参数Tab.1The parameters of manipulator jointand connecting rod

关节编号i连杆扭转角αi连杆长度ai关节转角θi连杆偏距di1(A)000d2(B)pi/2L1pi03(C)pi/2L2θ04(D)0L3pi/20

(1)

根据表1的参数,将i=1,2,3,4分别代入式(1),得到如下各关节变量的位姿矩阵为

(2)

2 路锥自动投收机械手运动控制仿真

在Matlab仿真环境中,利用Robotic Tools工具箱对路锥自动投收机械手进行运动学仿真,并以投放过程为例,分析机械手在实际投放过程中的轨迹规划问题,为后续实现机械手运动控制奠定基础.

2.1机械手运动学仿真

在Matlab中进行机械手运动学仿真[12-13],利用link和robot函数建立机械手对象,上述2个函数是进行机械手仿真的前提条件,并利用drivebot函数进行运动仿真.

(1) link函数功能为标定各关节运动参数,其使用的格式为L=link([α,a,θ,d],′standard′),其中α,a,θ,d分别为连杆扭转角、连杆长度、关节转角、连杆偏距;Sigma为关节类型,0为转动关节,1为移动关节;standard为采用标准的D-H参数.

(2) robot函数功能为建立各关节的连接,生成机械手模型,使用格式为robot(link,…).

(3) drivebot函数功能是利用关节位姿参数进行运动学仿真,使用格式为drivebot(r,q),其中r为机械手模型，q为各关节位姿.

机械手对象建模的程序代码如下:

L{1} = link([0 0 0d1],′standard′);

L{2} = link([pi/2L1pi0 0],′standard′);

L{3} = link([pi/2L2θ0 0],′standard′);

L{4} = link([0L3pi/2 0 0],′standard′);

R=robot(L,′机械手′);

drivebot(R,qr)

其中qr=[dpiθpi/2],d和θ分别为滑轨移动关节和旋转臂旋转关节参数,这2个关节参数可变动,其他2个关节参数保持不变.

取d=0,θ=0,由式(2)可计算得机械手末端D的变换矩阵为

(3)

利用语句T1=fkine(R,[0pi0pi/2])可得所建机械手末端位姿正解为

与式(3)计算结果相符,由此说明所构建的运动学方程和模型正确可用.

2.2基于五次多项式插值的机械手运动轨迹规划

2.2.1机械手运动规划设计

鉴于所研究的机械手运动轨迹过程为点到点的运动,运动过程中需要规避部分区域(如车厢挡板等),且考虑关节空间优越性,故选择点到点关节的空间轨迹规划[14].

轨迹规划方法一般是指机械手起点位置和终点位置之间用多项式函数来“内插”或“逼近”设定的路径,并使之满足在运动过程中需要的位移、速度和加速度等约束的要求[15].通过光滑时间函数对轨迹规划结果进行分析,经过大量实验发现,采用五次多项式函数插值法取得较好效果,保证了各关节角位移、速度、加速度曲线的连续性和平滑性.

2.2.2机械手运动轨迹规划需求分析

机械手由2个运动关节组成:滑轨和旋转臂.路径规划按2种控制模式分别进行了仿真分析:一种为解耦控制,即在满足控制要求的情况下拆分两运动关节,使每个关节运动没有交错;另一种为协调控制,即让机械手两关节协调配合运动,实现运动路径最优,时间最短.

以路锥投放过程为例,分别研究采用解耦控制和协调控制规划机械手运动路径,并进行比较分析.

路锥自动投放过程可看作将路锥从车内运送至车外,考虑到路锥运动过程中存在的干涉及其他实际因素,机械手末端运动轨迹并不是一个起点到终点的简单轨迹,还需设置若干中间节点.

路锥投放流程的固定节点设置如下:① 起点.旋转臂BC成水平朝里状态,则起点关节空间矢量qz=[0pipipi/2].② 路锥投放点.旋转臂BC成斜向下60°,则投放点关节空间矢量qr=[-30pi-pi/3pi/2].③ 终点.即第2个周期路锥被拾取的位置,其关节空间矢量qf=[-5pipipi/2].

2.2.3机械手运动轨迹规划仿真分析

(1) 机械手解耦运动控制仿真

一个投放控制周期为:滑轨上升→转臂旋转→滑轨下降至投放点→转臂旋转至终点.

为此设置一个中间点A,即为机械手保障不触碰下一个路锥的情况下,滑轨需上升最短距离的位置,此点的关节空间矢量qm=[55pipipi/2].

(2) 机械手协调运动控制仿真

投放控制周期中滑轨和转臂在大部分时间可同时运动,为此设置2个中间节点:

中间点B:设为滑轨单独上升20 cm,从该中间点后,滑轨和旋转臂开始动态配合完成投放过程,此点的关节空间矢量qm1=[20pipipi/2].

中间点C:此点滑轨开始下降(运动上限点),即此点的关节空间矢量qm2=[40pipipi/2].

在Matlab环境中,对机械手投放轨迹进行规划仿真,机械手末端在路锥投放周期内的运动轨迹如图3所示,两运动关节运动参数曲线分别如图4和图5所示,相关运动参数对照如表2所示.

表2 路锥投放周期滑轨和旋转臂运动参数对照表Tab.2 The comparison Table of motion parameters between slide and turning arm in the road cone delivery cycle

图3 路锥投放周期机械手末端投放过程运动轨迹Fig.3The placement process trajectory of robot tipin the road cone delivery cycle

(1) 投放效率:相比解耦控制,对机械手采用协调控制,其末端运动轨迹的路径明显缩短,路锥投放周期从7.8 s缩短至6.0 s,路锥投放效率提升23.08%;

(2) 关节运动平稳性:相比于解耦控制运动曲线,对机械手进行协调控制时,滑轨和旋转臂的运动加速度曲线更加平缓,即机构运动更加平稳,可有效减小路锥高速投放过程中出现脱落现象的几率.

图4 路锥投放周期滑轨关节运动曲线Fig.4 The motion curve of slide joint in the road cone delivery cycle

图5 路锥投放周期旋转臂关节运动曲线Fig.5 The motion curve of turning armjoint in the road cone delivery cycle

由以上仿真结果显见,采用协调控制方案可以精确、平稳地控制两运动关节完成优化的机械手末端运动轨迹,最终实现路锥的定点自动投放任务.

同理,机械手回收过程各关节运动曲线与投放过程运动曲线相似,在此不做赘述.

3 路锥自动投收机械手控制系统设计

滑轨和旋转臂的运动由步进电机驱动,基于仿真研究结果,提取滑轨和旋转臂运动参数作为对应步进电机的速度设定值,可实现对路锥自动投收机械手的运动控制.因此,机械手运动控制系统开发核心是步进电机的驱动控制板及控制程序设计.

3.1机械手控制系统总体方案设计

控制系统包含信息检测单元、驱动控制单元、人机交互监控单元及电源管理单元,如图6所示.

(1) 信息检测单元:实时采集旋转臂、滑轨位置信号、取锥信号(利用压力传感器测定待取路锥).

(2) 驱动控制单元:输出旋转臂电机和滑轨电机的PWM波驱动控制信号、电磁铁得/失电控制信号.

(3) 人机交互监控单元:实现系统参数设置、工况实施监测与路锥投收远程操作功能.

图6 控制系统总体框架Fig.6 The overall framework of control system

(4) 电源管理单元:为所有电机、传感器等电子元器件正常工作提供稳定电源,并保持系统在非工作阶段处于低功耗状态.

3.2机械手控制系统硬件设计

基于STM32F103ZET6主控器构成的机械手控制系统如图7所示.

机械手控制系统包含如下关键模块:① 最小系统,处理各种信息;② 电源管理模块,系统电源电压调节;③ 通信模块,与上位机建立通信机制,处理各种控制命令;④ 信号采集模块,采集压力传感器、限位开关等信号;⑤ 电机控制模块,电机驱动、停止,编码器信号输入、捕获;⑥ 继电器控制模块,电磁铁、刹车、系统电源等电源通断控制;⑦ 外围电路,状态信息的存储.

图7 基于STM32系列的机械手控制系统结构Fig.7 The manipulator control system structure based on STM32

3.3机械手控制系统软件设计

综合考虑操作可靠性及工作稳定性,控制系统软件基于uCOS -Ⅱ操作系统,并将其嵌入到ARM中.为提高控制实时性,系统建立了多任务运行机制,通过提高CPU利用率,减少系统完成任务所用时间,方便应用程序的设计和维护.

针对机械手运动控制,系统软件main函数中集成8个任务,分布如图8所示.其中任务1优先级别最高,任务1至任务8优先级别依次降低.

图8 系统任务分布Fig.8 The system task distribution

(1) 系统初始化:控制机械手运动至初始位置.

(2) 上下位通信数据处理:完成控制命令的接收与转化、状态量的设置与反馈.

(3) 传感器信号处理:完成压力传感器、限位开关信号的采集与处理.

(4)停止结束:完成一个工作周期停止工作以及急停工作.

(5) 滑轨上行:完成滑轨电机正转转速调制.

(6) 滑轨下行:完成滑轨电机反转转速调制.

(7) 旋转臂正转:完成旋转臂电机正转转速调制.

(8) 旋转臂反转:完成旋转臂电机反转转速调制.

3.4机械手运动控制系统调试

某型路锥自动收放车及机械手测试平台如图9所示.基于实车平台,以路锥投放过程为例,对机械手运动控制系统及其功能开展性能测试及验证.在路锥投放过程中,编码器反馈的速度信号通过串口反馈至上位机,可实时观察到各执行机构实际运动轨迹曲线,如图10所示.

图9 机械手实车试验Fig.9 The real vehicle test of manipulator

图10 机械手投放周期执行机构速度曲线Fig.10 The velocity curve of actuating mechanism in the road cone delivery cycle

图10所示两机构运动速度曲线与图4和图5中协调控制方案的关节运动速度变化趋势基本一致.由于在起始时刻,电磁铁得电取锥耗时0.2 s,仿真中尚未考虑该过程,故相对于计算机仿真速度曲线,实际速度曲线整体向右偏移0.2个单位时间.实车测试结果表明,路锥自动投放效果较好,且投放时间满足控制要求.

4 结论

(1) 基于实车测试平台,机械手轨迹规划实际试验结果与计算机仿真结果吻合较好,所提出的机械手建模方法及轨迹规划方法有效,可以满足路锥自动收放作业要求.

(2) 设计的机械手运动控制系统可实现路锥的自动投收作业,工作性能稳定、可靠.

(3) 该研究着重于机械手运动轨迹规划及控制系统实现,特别是对路锥自动收放装置设计与实现有着重要参考意义.

[1] 杨京帅,孙正一,王江兰,等.交通规则约束下的道路交通安全系统稳定性分析[J].中国公路学报,2015,28(1):95-101.

YANG J S,SUN Z Y,WANG J L,et al.Stability analysis of road traffic safety system under traffic rules[J].China Journal of Highway and Transport,2015,28(1):95-101.

[2] 吕能超,李泽,黄珍,等.高速公路事故现场隔离路锥布设车引导系统研究[J].交通信息与安全,2017,35(3):11-18.

LYU N C,LI Z,HUANG Z,et al.A study on a guide system of automated cone placement vehicles for isolation of traffic accident scenes on freeways[J].Journal of Transport Information and Safety,2017,35(3):11-18.

[3] 张卫华,谢晓琳,缪子英,等.资源共享条件下高速公路应急救援资源调度方法[J].中国安全科学学报,2011,24(5):170-176.

ZHANG W H,XIE X L,MIAO Z Y,et al.Method of scheduling expressway emergency rescue resource under condition of resources sharing[J].China Safety Science Journal,2011,24(5):170-176.

[4] LYU N C,LI Z,LUO Q H,et al.Research and design of road cones vehicle for traffic incidents disposal[C]// 16th COTA International Conference of Transportation Professionals(CICTP).2016:1777-1785.

[5] BOBROW J E,DUBOWSKY S,GIBSON J S.Time-optimal control of robotic manipulators along specified paths[J].International Journal of Robotics Research,1985,4(3):554-561.

[6] MACFARLANE S,CROFT E A.Jerk-bounded manipulator trajectory planning:design for real-time applications[J].IEEE Transactions on Robotics and Automation,2003,19(1):42-52.

[7] 徐海黎,解祥荣,庄健,等.工业机器人的最优时间与最优能量轨迹规划[J].机械工程学报,2010,46(9):19-25.

XU H L,XIE X R,ZHUANG J,et al.Global time-energy optimal planning of industrial robot trajectories[J].Journal of Mechanical Engineering,2010,46(9):19-25.

[8] CHEN H,NI Y B.A control method research of parallel mechanisms[J].Advanced Materials Research,2013,619:51-55.

[9] 解则晓,商大伟,任凭.基于Lomé曲线的Delta并联机器人拾放操作轨迹的优化与试验验证[J].机械工程学报,2015,51(1):52-59.

XIE Z X,SHANG D W,REN P.Optimization and experimental verification of pick-and-place trajectory for a delta parallel robot based on Lomé curves[J].Journal of Mechanical Engineering,2015,51(1):52-59.

[10] 吕能超,李泽,罗齐汉,等.交通事故现场隔离路锥车的总体设计与研究[J].中国工程机械学报,2016,14(3):233-238.

LYU N C,LI Z,LUO Q H,et al.Study and design of road cones vehicle for accidents disposal on site[J].Chinese Journal of Mechanical Engineering,2016,14(3):233-238.

[11] 项焱华.某型路锥自动收放机械手运动控制研究与实现[D].武汉:武汉理工大学,2016.

XIANG Y H.The research and implementation of a road cone automatic retractable manipulator motion control[D].Wuhan:Wuhan University of Technology,2016.

[12] HUANG P,XU X,MENG Z.Optimal trajectory planning and coordinated tracking control method of tethered space robot based on velocity impulse[J].International Journal of Advanced Robotic Systems,2014(11):1-5.

[13] GALICKI M.Real-time constrained trajectory generation of mobile manipulators[J].Robotics and Autonomous Systems,2016,78:49-62.

[14] CHOI T Y,PARK C H,DO H M,et al.Trajectory correction based on shape peculiarity in direct teaching manipulator[J].International Journal of Control,Automation and Systems,2013,11(5):1009-1017.

[15] SHAREEF Z,TRCHTLER A.Joint selection criterion for optimal trajectory planning for robotic manipulators using dynamic programming[J].IFAC Proceedings Volumes,2014,47(3):6025-6031.

Trajectoryplanninganddesignofcontrolsystemforroadconeautomaticretractablemanipulator

HUANGZhen1,XIANGYanhua2,LIZe3,LÜNengchao4

(1.School of Automation,Wuhan University of Technology,Wuhan 430070, China; 2.GDU Technology (Shenzhen) Co.,Ltd., Wuhan 430074, China; 3.Wuhan YCIG iLink Technology Co.,Ltd.,Wuhan 430074, China; 4.National Engineering Research Center for Water Transport Safety,Wuhan University of Technology,Wuhan 430063, China)

Taking 2-DOF road cone automatic retractable manipulator as the research object,trajectory planning method in operating and joint space under road cone automatic retractable are researched.Firstly,combined with practical requirements,the structure of manipulator is designed and motion model is established based on D-H parameter method.Analysis of robot trajectory requirement,the motion trajectory of manipulator is planned by quantic polynomial method,decoupling control and dynamic coordination control is considered,respectively.The motion trajectory of point to point is defined through a set of critical path points.In the environment of Matlab,taking the road cone automatic delivery as an example,simulation and analysis of manipulator trajectory planning are developed.The control system of road cone automatic retractable manipulator is designed;the control process of manipulator motion is conducted based on the results of simulation.Finally,based on the real vehicle test platform and plenty of experiment,the result of trajectory planning method and practical work for manipulator is proved.The results show that trajectory planning method and control system can meet the demand of road cone automatic retractable,and have a better effect.

road cone automatic retractable; manipulator; trajectory planning; simulation; control system

TH 112

A

1672-5581(2017)04-0283-08