基于ROS的电动汽车充电操作移动机器人系统

徐建明，蔡奇正，马益普

(浙江工业大学 信息工程学院，浙江 杭州 310023)

随着汽车排放标准的提高和限行限牌措施的日益普及，在新时期能源政策的扶持下，我国电动汽车市场份额不断增加，预计在2025年前后年销售量将达到300万辆[1]，充电桩及配套设施的建设也将得到较为完善的发展。目前，电动汽车充电主要靠人工完成，存在充断电不及时[2]，不利于有限充电桩资源的合理共享[3]等问题。随着机器人技术的变革，机器人处理复杂任务的能力得到了长足发展，因此可以将人工插拔充电枪的工作交由机器人完成。加快以机器人为基础的智能充电设备的研发，可以更加高效地利用有限的充电桩公共资源，对电动汽车的推广和政策落实具有重要意义。考察国内外充电设备，发现已有电动汽车充电操作是利用机械臂完成的，如特斯拉公司研发的蛇形电动汽车充电操作机器人[4]，大众公司基于KUKA机械臂的E-smart Connect充电操作机器人[5]以及Walzel等[6]基于手眼系统开发的充电操作机器人。这类充电操作机器人存在移动困难、工作空间固定、可服务电动汽车的车型款式少等问题。开发一款带有充电机械臂的自主导航移动机器人以完成充电任务，将会有广阔的应用前景。

移动机器人系统涉及的技术涵盖面广、难度大，包括SLAM[7]、路径规划和路径导航等机器人应用技术[8]，也包括多机协同、人机交互[9]等技术，如何把功能模块低耦合高内聚地整合至系统中是机器人系统研究领域的一大难点[10]。ROS是近几年来日渐被机器人研发人员青睐的一种开源机器人操作系统，具有分布式、弱耦合的节点框架，在设计机器人模块时可以不依赖特定的编程语言，可以利用社区资源简化开发过程中的代码重复编写，提高开发效率和代码复用率[11]。为了满足电动汽车充电操作移动机器人的一系列需求，笔者以全向牵引舵轮式AGV[12]为基础，利用ROS开发平台，融合带视觉功能的充电机械臂，设计出一种充电操作移动机器人。对充电操作移动机器人进行运动学分析，构建了完整的机械臂与底盘实际位置坐标转换关系[13]，完成了针对目标位置的系统移动方案，运用TCP Socket通信[14]实现了完整的车臂协同操作，最后完成了在该实验平台的充电操作实验。

1 控制系统架构设计

1.1 硬件系统架构

充电操作机器人的硬件大致可以分为两个模块，即充电机械臂模块和移动底盘模块。充电机械臂模块完成充电枪抓取与拔插，主体是大族六自由度协作机器人Elfin5，在其末端装配有力传感器和工业摄像机，用以实现对拔插姿态的识别和视觉定位。充电操作机械臂末端执行结构如图1所示。末端执行结构参考部分已有的机械臂结构设计[15]，在支撑桁架上装有1 组轴间距为70 mm的迈德威视8 mm焦距高清工业摄像头作为视觉模块。机械臂末端轴向上安装1 个六维力传感器，可以感知拔插充电枪过程中的接触力。在此传感器组所连接的安装平台上安装通用快接充电枪接口，通过气动吹顶活动钢珠，锁死或解锁快接头，实现充电枪的快接快拆，以达到让少量充电操作移动机器人匹配多套充电枪组，服务更多数量的待充电电动汽车的目的。

图1 机械臂末端执行模块机械结构图

移动底盘模块是一个自主研发的AGV，可以实现机器人自主导航，移动底盘采用双舵轮对角驱动，舵轮由牵引电机和转向电机组成，通过RoboteQ双通道驱动器驱动。定位传感器选用了Sick-lms111激光雷达、IMU惯性测量单元和增量式编码器。移动底盘的主控单元是一个搭载Intel赛扬处理器的工控机，用于接收调度信息、处理传感器回传数据并规划运动。外部客户设备通过局域网连入系统后，可以通过上位机配置和调试移动底盘和机械臂控制系统。控制系统硬件结构如图2所示。

所用硬件设备选型参数见表1。

表1 硬件设备选型参数表

1.2 软件系统架构

本系统在软件上是一个分布在移动底盘、上位机和机械臂系统中的分布式网络，可以按照层次划分为交互层、运动规划层和运动控制层，其结构如图3所示。

图3 控制系统软件架构图

相较于CodeSys这类商用控制平台[16]，ROS系统开源程度高、易用性强。自下向上，运动控制层通过ROS-IgH组件将EtherCAT主站封装在ROS的一个节点内[17]，并在Linux内核的基础上增加Xenomai实时性补丁[18]，确保EtherCAT主站可以利用线程共享内存方式与其他节点进行数据交互[19]，将运行周期压缩至1 ms，以弥补原生ROS系统的非实时操作缺陷[20]，同时在运动控制层内搭建了针对移动底盘的机械结构的逆运动学速度分解与正运动学里程计累计的功能节点[21]，实现将下发车体速度指令转换为舵轮电机指令的功能。充电机械臂模块完成力位置控制，在位置控制的基础上根据寻孔插孔过程中的阻力力矩的变化来分析充电枪与充电口的相对姿态，该部分工作由团队其他成员承担。基于ROS-move-base功能包设计了运动规划层框架，由一系列ROS节点为移动底盘提供建图、定位、导航和路径规划等服务，调用运动控制层的车体速度指令接口实现机器人的运动功能。在充电机械臂模块的运动控制层内，由视觉模块为机械臂提供视觉定位供寻孔插孔算法为机械臂规划轨迹。作为充电操作移动机器人两大独立模块的通信通道的交互层也提供了用户通过远程客户端访问系统进行调试的功能，笔者采用较为高效的TCP Socket方式建立了系统内的通讯。

系统层级控制流程与充电操作移动机器人的功能流程综合表述如下：待充电电动汽车停放完毕，向停车场管理系统提出充电申请，系统根据车型种类提取对应充电口位置信息并将其通过交互层发送至运动规划层，同时运动规划层根据传感器信息生成局部地图，结合全局地图和目标信息规划路径生成车体速度需求，并将需求发至运动控制层，运动控制层驱动移动底盘运动并获取底层传感器数据，统计里程计信息并反馈至运动规划层以提供定位参考，此时充电操作移动机器人携带充电枪移动至目标位置，到达待机地点后移动底盘通过交互层向充电机械臂的运动规划层发出动作请求，由充电机械臂的视觉模块进行目标充电孔位姿定位，如果定位误差过大就通过交互层向移动底盘申请微调，如果识别成功则利用寻插孔算法实现轨迹规划并将该轨迹下发至机械臂运动控制层，完成轨迹跟踪，此时充电机械臂进行充电枪插入动作，完成后断开充电枪快接接口并通过交互层发出完成信号，移动底盘接收到完成信号后，在运动规划层规划返回初始点待机的路径以供运动控制层跟踪，最后返回待机地点，充电服务响应流程如图4所示。

图4 充电服务响应流程图

2 控制系统运动控制层

笔者所研究的充电操作移动机器人系统框架中的移动底盘和充电机械臂模块相对独立，在运动控制上充分解耦合。全车坐标系示意图和实物图分别如图5(a，b)所示，为了避免碰撞、剐蹭等意外，在进行导航任务时，充电操作移动机器人的机械臂必须保持较小的空间姿态，车体运动时机械臂处于静止状态。当目标进入最佳工作空间时，车体减速并停止，此时机械臂进入对目标定位的工作模式。分别对底盘和机器人整体进行运动学建模，机器人整体以底盘的几何中心为基座标，构建连杆参数表如表2所示。

图5 全车坐标系示意图与实物图

表2 充电操作移动机器人连杆参数表

2.1 充电机械臂运动控制层

由图5(a)可得坐标系转换矩阵的一般表达式为

(1)

求得机械臂末端相对于底盘坐标系的转换矩阵为

(2)

(3)

求得机械臂末端相对{B}位姿为

(4)

式中：c12=cos(θ1-θ2)，s12=sin(θ1-θ2)。典型的机械臂充电操作过程可以视为在统一的坐标系下，结合视觉模块提取的目标位姿、机械臂末端位姿和力传感器信息，控制机械臂对目标进行拔插操作。

2.2 移动底盘运动控制层

移动底盘结构如图6所示，全向牵引舵轮对角布置支撑车体。这种动力配置方式相较于常见的两轮差速底盘和Ackermann底盘具有更多样化的机动能力，例如可以实现横向平移和行进间调整姿态角等动作，十分灵活。在移动底盘的运动控制层实现了对运动规划层下发的路径进行跟踪的功能。根据图6可以构建一个以底盘几何中心为原点的车体基座标参考系，由xo和yo方向确定的基坐标系为{O}、由xb和yb方向确定的机器人坐标系为{B}、舵轮坐标系为{Bi}(i=1,2)，其中i为第i号全向舵轮，下同。底盘相对于基坐标系{O}的姿态角为θb，角速度为ωb，正交分解得线速度vx和vy。舵轮转向轴心与车体中心的距离和夹角分别定义为lbi和αi，对应舵轮的速度和舵向角为vi和θi。

图6 底盘坐标系示意图

根据底盘在坐标系{B}中的牵引速度vx，vy和转角速度ωb可以推导舵轮速度vi和舵角θi，由几何关系可以推导得出机器人的运动学方程为

(5)

可以根据实际舵轮舵向角和运动速度推算里程计信息。

(6)

根据姿态角θb可以获得基坐标系与底盘速度的关系为

(7)

为了进一步研究机器人的运动学特性，假设车体各部件均为理想均匀刚体，且舵轮不发生打滑，系统采样周期为Tsample，可以推断出k时刻机器人相对于基坐标系{O}的实际位姿为

(8)

建立IgH EtherCAT主站与ROS Hardware共享内存的数据接口节点agv_bringup，使运动控制层负责与底层硬件设备连接，处理驱动电机和其他设备所需的多协议通信任务。agv_bringup节点随工控机开机一同启动，进行硬件资源接口的初始化、EtherCAT主站的初始化和设备状态映射的初始化，并将实物机器人的性能在ROS中抽象出虚拟模型，实现对上层通用指令的响应处理和反馈信息通用化，其流程如图7所示。节点收到上层运动规划层的速度控制消息/cmd_vel后，根据逆运动学方程式(1)，将此速度分解为两组全向转向舵轮的牵引速度和舵角，并通过主站下发到驱动器和电机，从电机编码器获得电机实际转速，经过运动学方程合并速度算出底盘实际速度。

图7 agv_bringup节点流程图

3 控制系统运动规划层及交互层

3.1 充电机械臂运动规划层

充电机械臂主要的功能是双目视觉定位和寻孔插孔算法的实现，双目视觉定位原理示意如图8所示。双目视觉模块安装于摄像头桁架，通过视觉工控机对双目图像进行预处理，经过标定的双目相机对目标进行位姿定位。根据双目标定原理可以将充电插座视觉中心拟合为点P，在左右双目相机坐标系中表示为PL和PR，建立坐标关系为

图8 双目视觉定位原理示意图

(9)

将双目相机中左相机坐标系作为参考坐标系，右相机相对于左相机的平移变换矩阵和旋转变换矩阵可以表示为R和T，PL和PR之间的转换关系为

PL=RT(PR-T)

(10)

由式(9，10)得平移变换矩阵和旋转变换矩阵为

(11)

利用这种标定关系可以在相机坐标系中获得目标点的坐标，结合机械臂坐标的转换关系就可以获得相基底坐标系坐标。

寻孔插孔算法使充电机械臂末端工具组即充电枪以一个面以法向方向为固定姿态进行装配，需要通过视觉定位规划出靠近目标的轨迹并通过后续寻孔轨迹的走动使充电枪在该面内与目标微弱接触，根据接触力数据判断充电枪与目标孔的相对姿态并进行微调，插孔算法则是在找准孔位后生成插入充电孔的力位置规划，防止力过大破坏设备或是力太小无法插入。

通过对六维力传感器在z方向上的数据分析获得寻孔与插孔的完成标志，将寻孔与插孔的运动规划分为两个阶段，在尚未找准孔洞的寻孔阶段，末端执行模块即充电枪以平行于充电插座的方向贴靠装配平面平移，由于此时紧紧贴靠，可在z方向上获得一个10 N左右的接触力，当寻准孔洞时，失去紧贴而出现z方向上力跳变至0 N左右的现象，此为寻孔与插孔阶段的分界。而插孔阶段结束的标志则为由于充电插头与插座完全贴紧而使机械臂装配力与力传感器大小一致，在z方向上可以获得一个接近120 N大小的力读数，以此为判断依据，可以判定充电枪插入完毕。

3.2 移动底盘运动规划层

ROS系统位于电动汽车充电操作移动机器人的移动底盘上，此部分的运动规划层的主要功能也可以看作导航规划。运动规划层通过定制ROS move_base实现运动规划功能，move_base是一个相对开放的功能包，拥有丰富的接口，可以根据需求选用或自定义导航算法。运动控制层需要以机器人实时位置为基础，通过全局路径规划器和局部路径规划器实现整个电动汽车充电操作移动机器人系统的路径规划，运动规划层的框架如图9所示。

图9 运动规划层框架图

运动规划层的流程如图10所示，根据移动底盘的安全半径，对全局地图进行安全区域膨胀，并生成对应的全局代价地图。先通过全局路径规划器在全局代价图上生成全局路径，局部路径规划成功后将局部路径通过/local_path消息发送至轨迹跟踪控制器，最后规划车体速度。电动汽车充电操作移动机器人配备了多种传感器，通过一个多传感融合节点实现多话题订阅，利用加权平均算法实现对数据的整理，对几种定位数据在采样周期内求取均方差并根据均方差调整权值，根据权值获得加权平均位姿定位结果并将其作为里程计信息，最后再封装成新的/odom话题发出。节点算法如图11所示。ROS move_base默认框架采用传统A*算法实现导航全局路径规划。A*算法是一种启发式搜索算法[22]，通过构建代价函数累计当前已有消耗和对于目标点的预估消耗，选取总消耗最低的节点作为子节点，重复迭代直到到达目标节点且路径最优，是一种智能导航算法。

图10 运动规划层工作流程图

图11 多传感融合位姿节点流程图

针对电动汽车充电操作移动机器人的机械结构特点，对A*算法在转弯半径上进行代价估算，选取转弯尽可能少的适合底盘的路径，可以表达为

F(n)=H(n)+G(n)+T(n)

(12)

式中：F(n)为代价函数在n节点上的代价值；H(n)为在n节点预估将要计算的代价值；G(n)为n节点已经累计的父系节点代价值；T(n)为n节点与父节点之间转向关系的奖惩代价调整值，转角越小其值越小，转角越大其值越大。采用这种改进算法可以减少规划路径中转角过大的转向。全局路径算法节点如图12所示。

图12 全局路径规划流程图

在局部规划器中，选择动态窗口法(DWA)进行局部避障[23]并配合激光雷达的实时反馈数据，具有高实时和可靠的特点。DWA算法的实现逻辑是在一个小范围的代价地图和时间窗口内，叠加全局地图和激光扫描获得的实时障碍数据，并估计待发送导航数据在下一时刻作用于机器人的位姿结果，选择最优的速度下发，从而实现对全局路径规划的实时局部修正。由于时间和空间窗口内障碍物数据消除频率较慢，因此调整局部地图的时间与空间参数，使移动底盘的正常运动不会被过多干扰且不与障碍物冲突。

3.3 交互层

交互层是模块间交互的主要通道，笔者基于Qt改进了可视化调试软件的设计，通过Ros-Qt插件建立Qnode，开设了ros系统和qt界面的数据接口，能够实现调试过程中的节点设置、功能操控和读数呈现等功能，可以在同一局域网下通过Socket传递充电机械臂的视觉识别结果到移动底盘运动规划层并回传机器人的运动状态。

4 实机实验结果及分析

在实验室环境下搭建了一个目标充电口并设置了待机位置。启动机器人后，初始化相关端口，启动agv_bringup节点，配置rviz界面和各传感器通信，完成配置后启动agv_navigation功能包的gmapping节点进行建图，将所建图作为导航依据存为全局地图，是后续导航的主要参考。gmapping建图结果如图13所示，图中1号圆圈为起始位置也是待机位置；2号圆圈为检验点，是用于检验误差校正算法有效性而设置的中间过程目标地点；3号圆圈为实际目标位姿；小圆圈为安装于墙壁上的目标充电口，黑色部分为障碍物，白色为实验室地面，灰色部分为未扫描位置。

图13 gmapping对实验室场景建图的地图

启动充电流程，通过目标数据库发送仿真充电口的位置坐标到移动底盘，导航agv_navigation功能包将利用全局地图形成全局代价地图，可以通过rviz观察到导航模块规划了到达该位置的路径并实现电动汽车充电操作移动机器人对此路径的跟踪。电动汽车充电操作移动机器人到达规定地点后，可以通过终端观察到已经向充电机械臂模块发送了视觉识别请求并进入等待。为了验证误差校准功能，第一次发送的位置并不是最佳位置，此时视觉模块会返回一个包含误差信息的“未能识别”消息，移动底盘根据回传的误差生成一个新的目标位置，随后导航至该位置，验证了该控制流程是可靠有效的。此时视觉模块接收到新的识别请求后进行识别，识别成功的消息发送至移动底盘，移动底盘进入机械臂动作等待状态，并使车轮抱闸，防止在机械臂动作时发生相对滑动。机械臂完成工作，松开快接接口并返回合适位姿时将发出“完成”消息，底盘接到该指令后结束抱闸，恢复车轮状态并返回初始位置。通过rosbag数据记录工具存取，获得odom和其他数据的存储和定位精度等信息。实验效果如图14所示。

a—电动汽车充电操作移动机器人在初始位姿的待机状态；b—移动底盘运行至检验位姿点的状态；c—电动汽车充电操作移动机器人通过视觉误差校正向指定目标点位姿移动；d—在目标位姿点进行充电枪插入服务；e—电动汽车充电操作移动机器人在目标位姿点上充电机械臂完成插入后断开充气快接接口并恢复到默认姿态的状态；f—电动汽车充电操作移动机器人返回值起始点。

电动汽车充电操作移动机器人处于图14(c，d，e)状态时可以通过特写画面观察到机械臂充电操作过程中的充电枪插头和插座的装配情况，结果如图15所示。

a—充电机械臂在识别到目标充电口的位姿后进入寻孔插孔过程的接近步骤；b—充电机械臂在寻得目标充电口孔位后插入插孔；c—充电机械臂完全插入充电口；d—充电机械臂完全插入充电口后断开气动快接接口，可以看到此时快接接口的钢珠处于收敛状态，可以分离充电枪，在实际应用中充电枪接有供电缆线；e—充电机械臂留下充电枪后回归待机姿态。

导航速度曲线如图16所示，移动底盘的3 个运动阶段之间的速度均为零，因此图中不体现。

图16 导航速度曲线图

对图13中标示出的3 个参考点的位姿信息进行观察，记录实验中连续10 次的位姿数据，与目标位姿比较，获得平均误差，对误差取标准差，结果如表3所示。由表3可知：实际位姿存在一定的累积误差，该误差具有明显上界。其原因是：为防止底盘为尽可能逼近目标点而进行反复挪动，发生打滑，导致定位失准，因此在控制位移的算法中添加了最小位移量，并设置了最大误差限制，在该误差限内不进行调整，只有累积误差超过误差限再进行调整。

表3 导航结果统计

观察机械臂模块对目标的识别情况，对充电孔识别结果如表4所示，底盘的低误差使机械臂模块对充电孔的识别具有较高的成功率，在一定范围内，两者可以互相补正误差，从而实现对插孔操作误差的修正。

表4 视觉模块对充电孔识别结果

5 结 论

研究了一种移动充电机器人系统，基于ROS机器人操作系统和视觉融合机械臂力位置控制设计开发了一套用于服务电动汽车的充电机器人平台，实现了自主导航，人机交互，模块协同等功能。控制系统和框架具有开源特性，在控制成本的同时，也具有易于维护和扩展的特性，实现了需求功能，对于类似的服务型工业移动机器人的开发具有一定的参考价值。