窨井盖更换机器人的运动学分析与其工作装置控制系统研究

吴卓良WU Zhuo-liang

（重庆交通大学车辆与机电工程学院，重庆400067）

0 引言

近年来，随着各个地区城市智能化进度的不断发展，电气与燃气运输、电信网络线路、给排水等线路管道铺设的增多，窨井的使用数量也越来越多。因此，在市政养护工作中，窨井盖的更换变得更为频繁。现存在的更换方式均为人工操作器械且还需不同工人进行相互配合方能完成，程序繁琐、更换时危险系数较大且效率不高。在窨井盖更换上，出现了由凯斯SR250 滑移装载机装配HS-57II 型井盖铣刨机附件构成的机械；也出现了由徐工筑路公司推出的XM50 型沥青路面铣刨机；另有VISTA 公司推出的集路面铣刨、井盖提拉一体的井盖铣刨机。窨井盖更换是市政养护工作的重点部分，现拥有的各种机械均为人工操作，无法达到智能化操作，高效更换的目的。因此，本文基于对路面铣刨之后进行井盖更换的方式设计一款具有较高稳定性、响应快速的两自由度窨井盖更换机器人。

1 机器人结构设计

窨井盖更换机器人主要包括机器人主体、末端执行器、定位器组成，如图1 所示。

图1 窨井盖更换机器人

末端执行器主要包括刀头、刀盘、提拉设备等，末端执行器是该机器人功能的直接体现，主要功能是对需更换井盖的路面进行旋转铣刨，配合窨井定位器进行定位并提拉井座。依据《城市道路工程设计规范》CJJ 37-2012：面层水泥混凝土的抗弯拉强度不得低于4.5MPa，快速路、主干路、重交通等不得低于5.0MPa。为此采用高强度碳钢合金刀头设计，共计18 枚，另刀头与刀片为可拆卸式设计，更换损坏刀头更具高效性与灵活性。同时为加大机器人灵活性及降低末端执行器重量，采用圆形刀片式切割。为适应不同尺寸窨井盖更换作业的要求刀片与刀盘之间为可调式伸缩结构设计。其原理是刀片通过高速旋转对路面进行铣削工作，到达铣削深度要求时，停止铣削，通过中部定位轴与定位器相互配合对窨井盖及其周边路面进行提拉作业，进而实行更换。

由于城市窨井盖的各式各样，其尺寸也因实际使用情况而不同，生活中常见的尺寸为600mm、700mm、800mm等，因此定位器的设计需满足能对各种尺寸窨井定位从而更换工作的要求，如图2 所示。

图2 窨井盖定位器

为了适应窨井圆形形状，采用三静臂呈Y 字形圆弧排列设计，各静臂夹角为120°，静臂与定位器中心及动顶头与静顶头均为可调式螺栓连接，依据窨井尺寸及井座高度进行调节达到定位稳固的工作要求。同时在静臂与定位器中心增设了弹簧接口，提高其工作时的稳定性。

为了能迅速移动至目的地进行快速更换工作要求，由图可知，机器人整体采用车辆式设计，末端执行器的驱动形式为液压马达驱动，实现对窨井盖周围路面的铣削功能；两动臂的驱动形式为液压缸驱动，相互配合实现对窨井盖的定位、井盖及井座整体的提拉。工作装置各关节均以铰接方式连接，另增添了备用人工操作室，预防机器人意外故障的发生。

2 机器人运动学分析

窨井盖更换机器人工作装置为大动臂、小动臂故而可简化为一个二自由度机器人。在机器人运动学中，分为正向运动学分析与反向运动学分析，另外描述各杆件间的相对关系时，通常采用D-H 参数法根据连杆坐标系{i}与连杆i 的位置关系，D-H 参数法有坐标系前置与坐标系后置两种，二者研究等价[1]，本文研究采用坐标系后置的方法，建立的连杆坐标系如图3 所示。

图3 机器人工作装置连杆坐标系

图中θ1表示动臂1 的关节角，θ2表示动臂2 的关节角；ai表示沿xi方向，zi到zi+1的距离，工作装置的各个连杆由连杆长度ai，连杆的扭转角αi，关节转角θi和连杆偏距di四个参数描述。

研究对象，窨井盖更换机器人工作装置的关节参数如表1 所示。

表1 工作装置D-H 坐标系参数表

机器人连杆坐标系{i-1}到坐标系{i}的齐次变换矩阵，表示为

对该机器人的正向运动学进行求解，计算公式为

在该机器人中，其工具坐标系与连杆2 的坐标系重合，因此

根据表1 中的数据及式（3）依次代入式（2）中，得到

其中，θ12是θ1+θ2的缩写；

cθ12=cos（θ1+θ2）；sθ12=sin（θ1+θ2）。由上式可知机器人末端的位姿，用向量[x，y，z，φ]T的形式来表示：

由此可知，机器人末端执行器的姿态角仅与两动臂转角有关。正向运动学的解是唯一确定的，即机器人的末端执行器的位姿是唯一确定的；反向运动学的解则往往具有多重解[2]，需根据特定情况而言选择其最优解。

3 液压臂控制系统设计

PID 控制器结构简单，在生产过程中是一种最普遍采用的控制方法冶金、化工、机械等行业中获得了广泛应用。

PID 的控制规律为：

其中，u（t）为控制器的输出，e（t）为控制器的输入，Kp为比例增益，Ti为积分时间常数，Td为微分时间常数。根据Kp、Td、Ti三种参数对系统相应的影响，经过反复调节整定，最终确定PID 的各控制参数。

经计算，大臂控制子系统开、闭环传递函数为

本文以大臂为例进行控制仿真分析，利用Matlab 对大臂闭环系统仿真，分别输入阶跃信号和正弦信号，分得到其响应曲线如图4、图5 所示。

图4 大臂系统单位阶跃响应曲线

图5 大臂系统正弦跟踪曲线

由图4 可得到峰值yp=1，即无超调，峰值时间tp=10.9s，调整时间ts=5.6s。由图5 可得正弦跟踪信号具有较大相位延迟，在短时间内不能达到预设值；由以上分析可知，大动臂控制系统在无PID 控制时响应时间较长且正弦响应误差较大，因此为缩短响应时间及提高响应精度，为此引入PID 控制器。

运用试凑法对PID 控制器参数进行整定[3]，通过大量实验仿真，对比之后得到了一组令人较为满意的系统PID参数：Kp=4.02，Ki=2.93，Kd=0.23。运用Simulink 建立加入PID 控制器之后进行大臂控制系统仿真模拟。

由图6 可得到，加入PID 控制器后大动臂系统阶跃响应曲线峰值yp=1，即无超调，峰值时间tp=1.73s，调整时间ts=1.3s，上升时间tr=0.79s，加入PID 控制器后的大臂系统响应时间及调整明显极大缩短；由图7 可得到加入PID 控制后，大动臂系统正弦跟踪信号基本没有衰减和相位延迟，能在较短的时间内达到设定值，PID 校正效果良好；综上所述，加入PID 控制器后的大动臂系统在保持良好的稳定性基础上，响应速度明显加快，跟踪输出能力增强，能快速达到设定控制要求。

图6 大臂PID 控制系统阶跃响应曲线

图7 大臂PID 控制系统正弦跟踪曲线

4 结语

针对目前窨井盖更换繁琐、效率低下设计一款便捷、灵敏的两自由度窨井盖更换机器人。采用D-H 参数法建立了机器人连杆坐标系并进行了运动学分析；运用PID 控制理论设计了机器人机械臂的控制系统并利用Matlab 仿真，结果表明了机器人工作装置控制系统设计的合理性及稳定性，为后续该类机器人的改进提供了参考。