洗车机二连杆机械臂定位控制策略及实现

储昭碧，王 昊，胡永新

（电气与自动化工程学院 合肥工业大学，合肥 230009）

洗车机二连杆机械臂定位控制策略及实现

储昭碧，王 昊，胡永新

（电气与自动化工程学院 合肥工业大学，合肥 230009）

介绍了一种新式的二连杆机械臂洗车机，相比传统洗车机，洗车效率高，占地面积小，存在着很好的市场前景。本系统基于台达DVP10MC11T总线型运动控制器，配合伺服电机，超声波传感器，控制精准，设计合理，通过MATLAB仿真，经过多次现场试验，达到了预期目标。同时，此二连杆机械臂控制算法对类似的控制系统具有一定的参考价值。

机械臂；运动控制系统；超声波传感器；伺服系统

0 引言

随着社会的进步，汽车在人们的日常生活中扮演了越来越重要的角色，洗车行业也是越来越发达，传统的人工洗车普遍存在着洗车时间长，洗车效率低等弊端，随着科技的进步，各式各样的洗车机也如雨后春笋一样层出不穷。现阶段市面上的洗车机基本上分为两类，一类是无接触式自动洗车机，一类是毛刷式自动洗车机，前者属于半自动产品，市面上的基本上只能洗车身两侧，对于车头车尾和车顶基本上无法洗到，仍需人工清洗；后者往往体积较大，需要的场地比较大，价格高，对于小型的洗车点来说不是很方便[1,2]。本洗车机是基于PLC的二连杆机械臂机构的洗车机，精巧的结构设计使得其要求的场地面积较小，对于车头车尾和车顶也能很好的洗到，效率高，存在着很好的市场前景。

1 系统结构及运行原理

1.1 系统组成

本系统是一套基于PLC的二连杆机械臂系统[3]，该洗车机由两个等长（1300mm）的主臂（大臂和小臂）一个L臂组成，其中L臂集成了风管、水管和清洗液管，负责具体的清洗工作。大臂小臂分别各自由一台伺服电机驱动，负责运动线路的控制，L臂可以自由升降，其升降杆由另一台伺服电机控制。同时，本系统配备了三个超声波传感器以实现对车身检测及机械臂运动路线的规划，三个超声波传感器一个用来车身侧面测距，一个用来车顶测距，一个用来对车身四角进行检测，以实现转角信号的发送。车身侧面测距传感器和车顶测距传感器装在L臂上，转角信号超声波传感器装在小臂末端。系统结构框图如图1所示。

图1 系统结构框图

实际工作中，通过超声波传感器[4～7]测出机械臂末端与车身侧面的距离d，通过与给定r进行比较得出差值e，同时与伺服电机中读出的角度一起是输入传给控制器，控制器进行计算得出两个电机的转速驱动机械臂运动产生x和y方向的位置，使得e趋近于0，同时使y方向的速度达到给定速度。

1.2 运行过程

当洗车辆驶入洗车机内部，安全确认完毕之后，洗车机开始运行，伺服电机驱动大小臂从而使L臂向车身侧面靠近，同时，L臂升降杆向车身顶部靠近，当检测车身侧面距离超声波传感器检测到L臂距车身200mm且顶部超声波传感器检测到升降杆距离车顶200mm时，升降杆停止向车顶靠近，L臂停止向车身靠近，根据超声波传感器反馈的距离进行PID控制，驱动大小臂转动使L臂开始以与车身始终保持200mm的距离围绕着车身运动，风管水管清洗液管配合工作，第一圈喷洒高效清洗液，第二圈空转一圈让清洗液充分反应，第三圈进行强力水洗，第四圈进行强力风干。车辆清洁完毕之后，机械臂回到初始位置，洗车结束。

2 系统模型搭建及仿真

2.1 系统模型搭建

图2为二连杆机械臂系统[8～11]的简化模型（点划线代表车身轮廓）。定义连接原点的臂为大臂，末端为小臂，此装置有两个关节和两个自由度，有两个驱动装置，其中两个臂的长度为L，机械臂末端到原点的距离为D，角θ为D边与x轴正向上侧的夹角，角α为大臂与x轴正向上侧的夹角，角β为大臂与小臂的夹角（小于180°），L为大小臂的长度。（x,y）为小臂末端的坐标，e为超声波传感器测出的距离d减去给定距离r的差值，ωα和ωβ分别为大小臂旋转角速度。默认在x轴负半平面分析，vx为L臂与车身垂直方向的速度，vy为L臂与车身平行方向的速度。在其他区域内只需更改方向即可，下文会详述。

图2 系统简化模型

2.2 公式模型推导

要实现通过控制两个大臂电机的转动来实现对其的控制，则需要将平面坐标的运动转换为大小臂电机旋转的角速度。

根据图1，由几何知识，可以得出：

（x,y）为机械臂末端的坐标，根据公式(1)，很容易得出：

机械臂末端的运动可以分解为x轴方向和y轴方向的运动，vx和vy可以由对x和y求导得出：

x方向的位移应该为实际机械臂末端到车身距离与给定距离的差值e，y轴方向的速度是认为设定的，即机械臂末端的运动x轴方向为减小e的运动，y方向为匀速运动，由此可得：

根据公式(4)，可以解出大小臂旋转的角速度。

从而可以计算出大小臂电机的转速，实现控制目的。

公式(5)计算出的ωα和ωβ分别为大小臂旋转角速度，将其除以2π即为两电机的转速，正负号代表正反转方向。α和β可以由伺服电机的编码器读出。对于x轴正半平面，vy为机械臂沿车身运动的速度（正值），vx为机械臂保持与车身200mm所需的速度（正值）；对于x轴负半平面，vy为机械臂沿车身运动的速度（负值），vx为机械臂保持与车身200mm所需的速度（负值）；对于y轴正半平面，vx为机械臂沿车身运动的速度（负值），vy为机械臂保持与车身200mm所需的速度（正值）；对于y轴负半平面，vx为机械臂沿车身运动的速度（正值），vy为机械臂保持与车身200mm所需的速度（负值）；使用时只需按机械臂的位置选择不同参数即可。

假设初始的夹角为α0和β0，机械臂末端到原点的距离为D0，位置坐标为（x0,y0）, 定义D边与x轴正向夹角为θ0。当前时刻的夹角为αt和βt，机械臂末端到原点的距离为Dt，位置坐标为（xt,yt），大小臂旋转的角速度为ωα和ωβ，定义D边与x轴正向夹角为θt。

显然，在时域中：

将公式(6)代入公式(2)中，可得出机械臂末端在坐标系中的坐标函数从而可以进行仿真，代入后有：

3.3 Simulink仿真

仿真[12]采用一条给定的线段对跟踪算法进行追踪性能分析，给定的线段函数如下：

为了使结果更加准确，所有参数按实际的大小设置，L臂的长度为1300mm，给定一个机械臂的初始位置，α=5π/3，β=π/3，对应到坐标系中的坐标为（1300，0）。

使用Simulink仿真后结果如下：

图3 仿真跟踪曲线

图4 机械臂与车身实时距离

起始位置为（1300，0），此时车身侧面位置为（1000，0），根据运行原理，机械臂应跟车身侧面保持200mm的距离，机械臂在x轴方向产生位移靠近车身使距离为200mm，同时y轴方向保持200mm/s的速度。

结合图3和图4可以看出，机械臂按照既定路线运行，且运行轨迹跟随性较好，在转角处的超调量较小，能满足实际需要，证明该算法的可行性。

4 系统硬件设计

4.1 硬件选型

本系统检测方面采用德国倍加福UC-2000-30GMIUR2-V15型超声波传感器，控制器方面采用的是台达DVP10MC11T总线型运动控制器，支持模块插接，该控制器将PLC可编程逻辑控制器和MC运动控制器整合到一起，通过高速总线（CANopen）可以控制高达16轴，可以实现高速精准的运动控制，伺服系统则选择了台达ASDA-A2-M型通讯型交流伺服驱动器搭配了20-bit(1280000 p/rev)增量型编码器电机则选择了同为台达公司生产的ECMA-C11010RS型交流伺服电机。

4.2 硬件连线

图5为系统硬件结构图，HMI与PLC通过485通讯进行连接，伺服驱动器ASDA-A2、DVP10MC11T和伺服电机通过网线连接，超声波传感器装在机械臂L臂上。

图5 系统硬件结构图

5 软件实现

本系统软件设计采用梯形图编程[13]，主要实现对误差信号的PID调节，电机转速和方向的计算和发送，对各个阶段的运行状态进行判断从而选择对应的阶段参数进行转速及方向的计算。使用梯形图编程，逻辑清晰，易读。程序流程图如图6所示，图中虚线表示PLC循环扫描机制，图7为运行流程图。

6 实验结果

现在市面上传统的自动洗车机，洗一辆车的平均耗时为10分钟左右[14]，而本款洗车机设计的沿车身运动的速度为200mm/s，现场测试时，使用的车型车身尺寸为5018×1866×1459，从系统启动到结束共耗时400s，低于传统洗车机耗时，效率更高。

图6 程序流程图

图7 运行流程图

7 结束语

本文给出了一套基于二连杆机械臂的洗车机运动控制系统的硬件组成和软件设计，经过现场实验测试，其跟随性好，效率高，根据行业市场统计，目前市面上传统的自动洗车机占用空间至少为7×3×4（m），而本洗车机所需空间仅为5.8×2.9×3.1（m），相比传统自动洗车机占用空间更小，具有一定的市场和推广价值，同时，也对类似的控制系统有一定的参考意义。

[1] 匡伟民.基于SIEMENS S7-200的接触式通道洗车机控制系统设计[J].轻工科技,2013,(07):94-95+128.

[2] 储昭碧,李健,孔艳,冯小英.基于S7-1200的全自动洗车控制系统的设计[J].电气传动,2016,(09):62-65.

[3] 王耀南,高小龙.基于CANopen的机械臂控制系统研究[J].控制工程,2016,23(1):1-5.

[4] Sahdev Kumar,Hideo Furuhashi. Long-Range Measurement System Using Ultrasonic Range Sensor with High-Power Transmitter Array in Air[J].Ultrasonics,2016.

[5] 蔡磊,周亭亭,郭云鹏,陈素芳,吴汉帮.基于超声波定位的智能跟随小车[J].电子测量技术,2013,(11):76-79+105.

[6] 金英连,王斌锐,方水光.未知环境下柔性二连杆机械臂的自适应阻抗控制[J].中国机械工程,2014,(17):2351-2355.

[7] L. Koval,J. Vaňu☒,P. Bilík.Distance Measuring by Ultrasonic Sensor[J]. IFAC PapersOnLine,2016,49(25).

[8] 李明,程启明,陈根,王鹤霖,邓亮.永磁同步伺服电机二自由度控制[J].电机与控制应用,2014,(10):1-5+25.

[9] Hongyan Wang,Lihua Tang. Modeling and Experiment of Bistable Two-Degree-of-Freedom Energy Harvester with Magnetic Coupling[J].Mechanical Systems and Signal Processing,2016.

[10] Shaogang Liu,Qianju Cheng,Dan Zhao,Lifeng Feng. Theoretical modeling and analysis of two-degree-of-freedom piezoelectric energy harvester with stopper[J].Sensors & Actuators: A.Physical,2016.

[11] Dong Wang,Tao Liu,Ximing Sun,Chongquan Zhong. Discretetime domain two-degree-of-freedom control design for integrating and unstable processes with time delay[J].ISA Transactions,2016.

[12] 姚振明,胡霞.基于MATLAB的永磁体同步电机模糊PID控制仿真[J].安徽理工大学学报(自然科学版),2016,(06):18-21+46.

[13] 台达电子工业股份有限公司.台达DVP-PLC编程技巧:WPLSoft软件篇[M].中国电力出版社,2012.

[14] 李培丽.节能便携式洗车机的研制[D].内蒙古工业大学,2010.

Positioning control strategy and realization of two - link manipulator for washing machine

CHU Zhao-bi, WANG Hao, HU Yong-xin

TP273

：A

：1009-0134(2017)08-0153-04

2017-06-06

储昭碧（1970 -），男，安徽人，教授，工学博士，研究方向为现场总线网络、自动化系统集成等。