面向电动公交车的充电机器人感知与控制

李少雄，张黎明，曹筱欧，郝爽，徐志宏

(1.国网天津市电力公司 滨海供电分公司，天津 300450；2.国网天津市电子公司 城东供电分公司，天津 300010)

0 引言

充电是电动公交车运营过程中的重要辅助工作，人工操作劳动强度大、成本高，这促使充电作业向机械化和自动化发展。近年来，机器人已广泛应用于汽车行业，充电机器人研究成为国内外机器人领域的热点之一。采用机器人代替人工自动插接充电枪是电动公交车运营的未来趋势[1]。机器人在电动公交车停车位置不可能重复的情况下代替人工操作，必须能自主感知充电孔的位置，同时具有灵活、可靠的控制系统。

目前，已有学者将智能制造领域的技术应用于充电机器人感知与控制。姚安庆采用双目视觉感知定位充电孔，实验研究表明只在特定场景下具有可行性[2]。段峥祺研究了一种基于特征点匹配的充电接口感知定位算法，即通过控制三维运动装置使得采集图像与标准图像完全匹配，但实际充电枪与充电孔插接时，无法采集充电孔图像，该算法的可行性还需验证[3]。纪柱只研究了电动汽车充电口识别算法，没有根据充电机器人研究标定方法[4]。马立焱、王晓安采用超声波感知定位充电孔，需要改造充电孔并设置超声波接收探头，实用性不足[5-6]。孙成研究的充电孔感知方法需要添加外部特征，再应用单目视觉基于圆特征的位姿测量算法[7]。张辉等研究了基于曲率滤波和反向P-M扩散的充电孔视觉感知方法，但没有给出手眼标定方法，所得结果不能用于机器人控制[8]。与充电孔感知方法研究相比，充电机器人控制系统研究不足，仅施莹在研究双目视觉传感器感知充电孔的位置时，设计用于机器人自动插接轨迹规划实验研究的控制电路，尚不具有工程应用价值。

本文针对面向电动公交车的充电机器人，根据其工作过程研究感知与控制系统。

1 方案设计

面向电动公交车的充电机器人如图1所示。当接收到充电指令后，机器人从充电桩抓取充电枪，运动到电动公交车充电孔正前方，感知充电孔位置后将充电枪插入充电孔，充电桩启动充电。充电完成后，机器人拔出充电枪并送回充电桩。机器人需要配置合适的感知与控制系统才能满足应用要求。在感知方面，双目视觉如果不能同时拍摄到充电孔，则无法确定空间位置，而线激光扫描方法和3D视觉系统成本较高，综合考虑可靠性和经济性因素，机器人采用激光测距仪和单目视觉感知充电孔的位置。手动示教表明，对于同型号电动公交车和充电枪，只需感知充电孔在机器人坐标系中x、y和z轴的坐标，定位误差在0.5 mm内都能够成功插接充电枪；在控制方面，示教确定充电枪姿态后，机器人根据感知结果通过平移直线运动就可以将充电枪与充电孔插接。此外，在整个动作过程中，机器人还需与其末端的机械手、故障处理等协调控制才具有工程应用价值。

图1 充电机器人

充电机器人感知与控制系统结构如图2所示。系统各部分通过路由器构成控制局域网，触摸屏(HMI)用于供用户操作和查看工作状态，相机和工业控制计算机完成图像采集与处理，可编程控制器读取激光测距仪测量值、控制充电桩和机械手，同时与工业控制计算机协同控制机器人。

图2 充电机器人感知与控制系统结构

2 充电机器人感知

充电机器人感知即识别充电孔，再通过手眼标定与激光测距仪、机器人配合确定充电孔在机器人坐标系中的位置。

2.1 充电孔识别

机器人感知充电孔在Oyz平面坐标的前提是识别和定位充电孔在采集图像中的位置，本文采用模板匹配实现。充电孔的采集图像如图3所示。从图中可以看出充电孔的内部结构复杂，灰度特征不明显，特征显著的是充电孔直流正负极的边缘，采用基于边缘特征的形状匹配算法在可行性、抗噪性和定位精度方面优于灰度匹配。应用形状匹配首先需要创建模板，创建时首先在采集图像中应用交互式感兴趣区域(ROI)定义功能，创建包含直流正负极的矩形ROI，通过对角坐标确定ROI区域的大小，并从采集图像中截取ROI图像，再从截取图像中提取直流正负极的边缘。边缘检测的效果决定了充电孔识别的准确性。已有研究表明，Canny算子在信噪比、检测精度和抗噪声性能方面优于Roberts、Sobel、Prewitt等其他边缘检测算子，能够检测出较多、较细的边缘点信息[9]。本文应用Canny算子检测直流正负极的边缘，根据检测的边缘创建用于匹配的模板。创建模板时，图像金字塔级数与匹配速度、匹配成功率相关，金字塔级数值越大匹配速度越快，但匹配成功率较低。可权衡准确性和搜索速度的选择，使匹配算法在两方面均能被充电孔识别应用接受。由于实际电动公交车的充电孔不可能存在明显的旋转角度，模板可能发生旋转的范围就可以设置一个很小的范围，从而提高匹配速度。

感知过程中，从采集图像中通过形状匹配确定充电孔中心的像素坐标，需要选择合适的算法参数才能满足准确性和匹配速度的要求。需要确定的参数有最小匹配分值、金字塔级数、搜索精度控制，其中最小匹配分值越小，匹配时越容易识别，但为了防止充电孔误定位导致充电机器人或充电孔损坏，在保证匹配成功的情况下，应尽量增加最小匹配分值；搜索时金字塔级数一般使用创建模板时的级数，同时使用基于最小二乘法的亚像素精度匹配保证充电孔定位精度；由于实际充电孔旋转角度非常小，可以限定搜索角度范围加快搜索速度。在采集图像中，通过形状匹配定位充电孔中心的实例如图3所示。

图3 形状匹配结果

2.2 手眼标定

识别和定位获得的是充电孔在采集图像中的像素坐标，还必须通过手眼标定建立像素坐标系与机器人坐标系的对应关系。确定充电孔的位置坐标才能用于机器人控制，手眼标定是充电机器人感知的关键[10]。

手眼标定模型如图4所示，OR-xRyRzR为机器人坐标系，OF-UV为像素坐标系，P(y,z)是机器人坐标系中的点，Q(u,v)是P在相机成像平面的投影点，P(y,z)和Q(u,v)可以通过旋转和平移来转换，表示为

图4 手眼标定模型

(1)

通过3组不在一条直线的P、Q坐标，即可求解出旋转矩阵R和平移矩阵T。为了便于用矩阵运算来表示仿射变换，式(1)可写为

(2)

其中A为仿射矩阵。

标定过程中，通常用具有固定参考点的标定板和标定尖端，分别确定中心点的像素坐标和机器人坐标。但充电孔的位置内嵌于车身装饰盖板上，传统标定方法不具备可操作性，而充电机器人采用手部相机，实际标定时可以固定充电孔，保持相机拍摄姿态和距离不变，通过移动机器人和相机采用模板匹配获得多组不同的P(y,z)和Q(u,v)。为减小手眼标定误差，实际标定一般采用9组点[11-12]。控制机器人在Oyz平面内按“田”字型运动，移动到“田”字的9个交叉点，记录9个位置点的机器人坐标和匹配得到的模板中心像素坐标，手眼标定流程如图5所示。

图5 手眼标定流程

2.3 感知步骤

充电机器人将充电枪与充电孔插接前，感知充电孔位置的步骤为：

1)机器人移动到标准拍摄位置P0(x0,y0,z0)；

2)读取激光测距仪的检测值l，并判断检测值是否在量程范围内，如果超出量程，跳转到8)输出位置感知错误；

3)计算充电孔在机器人坐标系x轴相对示教时变化量Δx=l-l0；

5)相机采集图像并进行形状匹配，匹配成功，则确定充电孔模板中心的像素坐标(u,v)，匹配失败，跳转到8)输出位置感知错误；

6)根据仿射变换计算充电孔模板中心在机器人坐标系中的坐标(y,z)；

8)输出位置感知错误。

3 充电机器人控制

充电机器人运行过程中需要根据感知系统的输出，在线修改插枪动作的目标位置坐标才能将充电枪插入充电孔，和取枪充电、充完收枪机器人动作轨迹不同，在机器人控制器上实现过程控制程序灵活性不足[13]。PLC和IPC协同控制机器人的方案如图6所示，PLC程序决策动作执行、动作顺序，IPC根据相应动作向机器人控制器发送目标位置坐标，机器人控制器仅执行具体动作。

图6 机器人协同控制原理框图

3.1 机器人控制器程序

充电机器人取枪充电、充完收枪动作过程的运动轨迹可分解为一个或多个转出、转入、直线动作，机器人控制器程序只需根据PLC输出的执行条件执行动作指令，流程图如图7所示。轮询动作执行条件，条件使能时机器人执行相应的动作指令，动作完成后，机器人控制器输出完成信号反馈到PLC，PLC复位动作执行条件。

图7 机器人控制器程序流程图

3.2 PLC控制程序

根据实际充电机器人充电作业过程，PLC控制程序分为取枪充电、充完收枪两个部分。PLC程序采用状态转移法设计，根据运动轨迹将动作过程划分为若干状态进行顺序控制，每一状态对应一个动作，同时PLC置位标志位，通知IPC向机器人控制器发送当前状态的目标位置坐标，IPC发送完成后复位标志位。充电作业过程涉及的功能组件都向PLC返回执行结果，当PLC检测到当前状态的动作完成后，转移到下一个状态，直至过程结束。取枪充电过程如图8所示。机器人从充电桩抓取充电枪插入电动公交车的充电孔，包含20个状态，图中椭圆矩形表示机器人动作，除转入、转出动作外，其他动作都是直线运动。PLC程序互锁输出执行条件保证机器人动作的唯一性；机器人故障、动作轨迹规划失败或急停时，转移到机器人故障状态，发出报警，由人工介入处理；距离检测异常或视觉系统异常、充电桩故障时，程序输出故障提示和报警。

图8 取枪充电过程状态转移图

充完收枪过程如图9所示。控制机器人从电动公交车充电孔拔出充电枪送回充电桩，充完收枪过程中的状态名称定义和取枪充电一致的原因是机器人位置坐标相同，而且收枪时抓取充电枪的位置坐标是在取枪充电过程中确定的，区别仅在于具有相反的机械手携枪、无枪状态，这样可以简化IPC与机器人控制器的通信程序。此外，在取枪充电过程中发生距离检测异常或视觉系统异常时，自动跳转到充完收枪转入状态，将充电枪送回充电桩。

图9 充完收枪过程状态转移图

PLC程序还包括机械手逻辑控制、激光测距仪模拟量采集及距离计算、通信看门狗等功能，应用位逻辑、比较指令和定时器等，采用梯形图可以很容易实现。

3.3 IPC控制程序

IPC控制程序的功能是根据PLC工作过程的动作标志位与机器人控制器通信，发送动作轨迹坐标、充电孔位置坐标和查询机器人工作状态。IPC需要同时和PLC、机器人控制器通信，采用具有网络开发工具、支持多种底层网络协议的高级语言设计。

PLC支持OPC、TCP、MODBUS-TCP等多种协议，通过以太网通信，其中OPC通信无需设计底层通信程序，具有配置简单、使用方便的优点[14]。OPC服务器软件包含PLC驱动，通过添加变量和设置PLC元件地址就能实现读写PLC数据。在IPC控制软件中添加OPC客户端、创建绑定变量就可以与PLC中的数据建立对应关系。IPC程序中只需建立与PLC程序中动作状态标志位对应的共享变量，通过OPC服务器读写并同步更新。

机器人控制器提供基于TCP以客户端/服务器模式的通信控制方式，IPC需要向机器人控制器发送状态查询帧和控制命令帧，查询数据包括机器人当前坐标和工作状态，控制命令帧为修改目标位置坐标。

4 实验验证与应用测试

为验证充电机器人感知与控制系统的可行性和有效性，设计了如图10所示的实验平台。充电孔安装支架可以在底板上的槽中整体移动，槽的方向与机器人坐标轴方向一致，可以模拟不同的停车位置。

图10 实验平台

手眼标定时，先通过示教机器人得到充电孔的标准位置坐标为PI(338,1 227,791)mm，选择拍摄距离为400 mm，标准拍摄位置坐标为P0(182,1 240,650)mm，标准拍摄位置与充电孔位置的偏移量Δy、Δz分别为-13 mm、141 mm。设置“田”字交叉点间距为30 mm，手眼标定数据如表1所示，计算仿射变换矩阵A为

表1 拍摄位置手眼标定数据

序号机器人坐标(y, z)/mm像素坐标(u, v)/像素1(1 240.00, 650.00)(686.27, 474.95)2(1 270.00, 650.00)(516.56, 477.52)3(1 270.00, 680.00)(522.26, 301.78)4(1 240.00, 680.00)(680.33, 306.92)5(1 210.00, 680.00)(861.20, 308.04)6(1 210.00, 650.00)(855.46, 472.74)7(1 210.00, 620.00)(850.85, 647.18)8(1 240.00, 620.00)(691.69, 645.21)9(1 270.00, 620.00)(511.51, 641.93)

针对充电孔固定位置，机器人在标准拍摄位置10次感知的结果如表2所示。y、z轴坐标相对实际位置最大误差不超过0.5 mm，重复性能够满足要求。

表2 感知重复性测试数据

进一步，通过定量改变充电孔的位置验证感知准确性和控制可靠性，部分实验结果如表3所示。x轴的实际坐标误差较大的原因是充电孔端面与机器人Oyz平面不平行，人工无法准确测量实际坐标，但在标准拍摄位置，机器人通过激光位移传感器能够感知拍摄距离的变化量，并计算出目标坐标。结果表明，机器人能适应充电孔端面在三轴方向存在的微小偏转角。

表3 感知与控制测试结果

实验验证感知与控制方法后，充电机器人在某公交充电站现场试运行，如图11所示。统计表明，在充电孔感知成功时，PLC、IPC与机器人控制器都能协调自动完成充电动作过程；当停车距离超过激光测距仪的量程时，以及电动公交车停车位置超过规定范围、充电孔图像采集不完整时，机器人都能将充电枪送回充电桩并报警。其他失败的原因是停车时车身与停车线的夹角超过设计范围，与示教时偏差较大，相机光轴与充电孔端面法向之间的夹角过大，而视觉系统的检测阈值设置偏低，充电孔感知坐标误差超过允许范围，插枪卡阻导致机器人过载并保护性停机。

图11 充电机器人样机

5 结语

本文根据充电机器人的工作过程和控制要求研究了感知与控制系统。感知系统通过激光测距仪和眼在手上的单目视觉，与机器人配合感知充电孔在机器人坐标系中的位置。基于示教获得的标准拍摄位置、拍摄距离和充电孔标准位置，机器人通过激光测距仪感知充电孔x轴位置变化量，并调整拍摄位置，在采集图像中通过形状匹配、结合手眼标定感知y轴和z坐标。基于PLC和IPC的协同控制系统实现充电过程自动控制，PLC与机器人控制器采用IO输出动作条件、查询动作结果，IPC通过OPC读取动作标志位，并采用TCP向机器人发送动作目标位置坐标。实验和应用结果表明，充电孔感知误差不超过0.5 mm，控制系统功能完善、可靠性好，在电动公交车运营过程中具有较强的实用性。