基于现场总线的双臂巡线机器人控制系统 【技术领域】
本发明基于现场总线的双臂巡线机器人控制系统，涉及先进机器人制造技术领域，是在高压输电线路线上作业中使用的巡检系统的自动控制系统。利用分布式硬件结构组成整个机器人的控制系统，形成了适用于高压输电线路线上巡检、除冰等一系列作业的先进机器人控制系统。
背景技术
高压输电线路作为电力输送的主要方式，是国民经济的大动脉，其安全可靠的运行是社会生产和人民生活的重要保障。但是，输电线路长期暴露在日晒、雨淋、大风等恶劣气候条件下，极易发生断股、锈蚀、脱线等损伤，在某些天气条件下，如雨雪冰冻等，甚至可能由于线路结冰太厚造成输电线路断裂，直接影响电力的输送。因此，电力公司需要经常对输电线路及其设备进行检查，发现并修复缺陷，清除结冰，以保证可靠的电力输送。
目前，这些工作基本靠人工来完成，这样工作效率低下，且工作环境险恶，很难保证工人的人身安全。随着机器人技术，人工智能和现代机械设计的不断发展，设计一种可以代替人进行输电线路巡检和除冰的自动化装置有了可能。巡线机器人就是以此为出发点而设计的一款自动化装置。通过加装不同的功能模块，巡线机器人可以代替人工完成线路巡检和除冰等工作，从而减轻工人劳动强度，降低工作风险，提高工作效率，保证输电线路稳定可靠的运行。目前，国际上巡线机器人研究现状如下：
日本Tokyo Electric Power公司的Sawada等人研制了用于检测光纤复合架空线路的自主移动机器人，实际运行时安装在地线上，能跨越障碍物。当它碰到障碍物时，打开自身携带的弧形臂，臂的两端握住障碍物的两侧，构成一个导轨，本体顺着导轨滑过障碍物。跨越过障碍物后，自动将弧形臂折叠起来，收在本体的下方。机器人装有自行研制的驱动设备，具有自驱动能力。
日本法政大学的Hideo Nakamura等开发了电气列车馈电电缆巡线机器人，采用多关节小车结构和“头部决策，尾部跟随”的仿生控制体系，能跨越分支线、绝缘子等障碍物。机器人由六对左右对称、相互联结的小车组成，每个单体小车有两个电机，左右小车采用磁锁系统联结。当机器人遇到障碍物时，每对小车顺次将磁锁打开，机器人再改变两侧旋转关节的关节角，使左右小车分开；小车依次通过障碍物后，控制两侧旋转关节使左右小车合拢，磁锁再次锁紧，机器人恢复正常行走状态。
日本静冈大学的Tsujimura等人开发了一种架空通讯线缆巡线机器人。机器人设计成四臂仿生式结构，四只手臂分为前后两组，各自铰接在一起，分别固定在前后两个盘型连接器上，并通过皮带传动与电机相连。在运行时，由电机带动盘型连接器转动，每组手臂由于相互铰接的作用，其末端的就会产生弧形运动，在线缆上形成四臂交替行走的动作，从而实现机器人行走和障碍跨越。但这种机器人只能跨越等间距排列的障碍，对于不规则障碍和杆塔是无法跨越的。
美国TRC公司研制了一台悬臂自治巡线机器人，它采用三臂悬挂结构，能沿架空导线长距离爬行，执行电晕损耗、绝缘子、结合点、压接头等视觉检查任务，对探测到的线路故障数据预处理后，传送给地面人员。当机器人遇到杆塔时，利用手臂采用仿人攀援的方法从侧面越过杆塔。
武汉大学吴功平等研制了220kV单分裂导线双臂巡线机器人。该机器人结构紧凑，重量轻，采用丝杠连接两只手臂与本体，越障时通过丝杠联动实现两只手臂的交互滑移异位，再通过转动副越过障碍物。机器人具有较好的抱卡机构，在一只手臂越障时，可利用电力线的局部刚性，维持机器人本体的位姿不发生大的变化，进而保证各种动作的顺利完成。
中国科学院沈阳自动化研究所研制了沿500kV地线巡检机器人，机器人采用小车式结构，可以在两塔档间自主行走，不能跨越杆塔障碍物。利用分布式专家系统构建机器人控制系统，实现机器人运行的智能化。利用携带的摄像机或红外热像仪等检测装置，可以检测输电线、防震锤、绝缘子和杆塔等输电设备的损伤情况。实现了机器人和地面基站远程通讯，基站对机器人运行状态的远程控制。
【发明内容】
本发明的目的是在已有的先进机器人控制技术基础上提供一种基于现场总线的双臂巡线机器人控制系统，使用该控制系统能够安全可靠的控制巡线机器人的各种运动，并通过在机器人上挂载相应的设备，可以实现输电线路的自动巡检和除冰等作业，减轻工人劳动强度，降低工作风险，提高工作效率，保证输电线路的安全可靠运行。
为达到上述目的，本发明的技术解决方案是：
一种基于现场总线的双臂巡线机器人控制系统，包括机器人控制主机、运动控制单元、传感器单元、系统状态监测单元、检测设备控制单元和地面基站，机器人控制主机包括嵌入式PC104计算机、图像采集卡、无线网卡，是机器人控制系统的核心部分；
机器人控制主机和各单元分别经现场总线电连接，并通过现场总线相互进行实时通讯；机器人各单元都有独立的微处理器，能够对本单元信息进行预处理，并通过现场总线将预处理结果发送给机器人控制主机，控制主机也通过现场总线实现对各个单元的实时访问与控制；
其在运动控制单元，传感器单元、系统状态监控单元和检测设备控制单元中包括ARM(Advanced Risc Machines，高级精简指令机)控制板、FPGA(Field Programmable Gate Array，现场可编程门阵列)控制板和CPLD(Complex Programmable Logic Device，复杂可编程逻辑器件)伺服驱动板；
ARM控制板包括ARM9芯片、FLASH存储器、时钟电路、现场总线接口、PC104总线接口和RS232接口；其中，ARM9芯片分别与时钟电路、PC104总线、RS232总线、现场总线控制器电连接，现场总线控制器与现场总线电连接；并经地址总线、数据总线与FLASH存储器电连接；
FPFA控制板包括现场可编程逻辑器件FPGA、FPGA配置电路、时钟电路、模式选择电路、电源模块和PC104接口；其中，现场可编程逻辑器件FPGA分别与时钟电路、PC104总线、模式选择电路、电源模块、FPGA配置电路电连接；FPGA配置电路包括JTAG电路和PROM，其中PROM和现场可编程逻辑器件FPGA串行连接在JTAG电路上；现场可编程逻辑器件FPGA并经电机速度接口和电机方向接口与CPLD伺服驱动板9电连接；
CPLD伺服驱动板包括CPLD芯片、时钟电路、H桥控制电路、光耦隔离电路及过流保护电路。其中，CPLD芯片分别与时钟电路、现场可编程逻辑器件FPGA、JTAG(Joined Test Action Group，联合测试行动组)电路、H桥控制电路、光耦隔离电路电连接；H桥控制电路与直流电机电连接；光耦隔离电路与比较器输出端电连接，比较器两输入端，一接设定值信号，另一接驱动器电流；
传感器单元包括ARM控制板和多种传感器，ARM控制板完成对各传感器信号的采集与预处理，并通过现场总线实现与运动控制单元的实时通信；传感器主要有超声传感器、红外传感器、霍尔传感器、触点开关；
系统状态监控单元和检测设备控制单元都有ARM控制板及检测设备，ARM控制板用来采集系统及检测设备的状态并通过现场总线实现与控制主机的实时通信；检测设备包括机器人电源检测单元、系统运行检测单元和设备检测单元；
传感器单元采集输电线路信息并经过本单元微处理器进行预处理后，通过现场总线传送到机器人控制主机，控制主机综合传感器信息、系统状态和检测设备信息，做出动作规划，向机器人的运动控制单元发送控制命令，控制各关节电机的转动，实现机器人在输电线路上越障和行走，达到对高压输电线路巡检地目的；
机器人控制主机通过无线网络与地面基站进行实时通讯，地面基站操控人员依据信息做远程处理，给出指令，通过控制主机调动各单元，实现机器人在输电线路上越障和行走，完成高压输电线路巡检任务。
所述的基于现场总线的双臂巡线机器人控制系统，其所述控制系统采用分布式的硬件结构，各控制单元既相互独立又通过现场总线联系在一起，各单元信息通过现场总线发送到机器人控制主机，控制主机通过现场总线实现对各单元的实时访问与控制。
所述的基于现场总线的双臂巡线机器人控制系统，其所述机器人控制主机，作为机器人控制器的核心部分，主要完成行为层次上的越障规划、图像采集与传输、障碍信息融合与识别、各单元之间的组织管理等。所述的基于现场总线的双臂巡线机器人控制系统，其所述运动控制单元中，ARM控制板完成越障动作规划和各关节电机的协调运动，FPGA控制板和CPLD伺服驱动板完成电机的伺服控制和过流、过载保护。
所述的基于现场总线的双臂巡线机器人控制系统，其所述传感器单元，包括超声传感器、红外传感器、霍尔传感器、触点开关和负责信息融合的ARM处理器；其中，超声传感器阵列探测电力线上方和下方的障碍物，红外传感器阵列检测电力线与手爪的相对位置，霍尔传感器检测各运动关节的机械限位，触点开关用于其他方式障碍物检测失败后的障碍备用检测手段，所有这些传感器信息经过ARM处理器融合后得到初步的外部环境信息。
所述的基于现场总线的双臂巡线机器人控制系统，其所述系统状态监控单元能够实时监测PC104主机的工作状态，一旦发现PC104主机死机或重启，立即向运动控制器发出停止运动命令，避免因PC104主机故障而带来的危险。
所述的基于现场总线的双臂巡线机器人控制系统，其所述检测设备控制单元利用ARM9处理器采集各检测设备的工作状态并进行实时分析，将预处理的结果通过现场总线传送给机器人控制主机，控制主机综合各中信息通过检测设备控制单元的ARM9处理器对各检测设备进行实时控制。
一种所述的基于现场总线的双臂巡线机器人控制系统的控制流程，其包括步骤：
C)开启机器人控制主机，经现场总线对各单元初始化；
D)检测设备控制单元采集系统、设备信息，传感器单元采集传感器信息，处理后，经现场总线返回控制主机；
C)控制主机发出运动控制指令，综合各单元返回的信息ARM控制板作动作规划，现场可编程逻辑器件FPGA产生直流电机的速度、方向控制信号；
D)直流电机的速度、方向控制信号传递给CPLD伺服驱动板；
E)经过CPLD芯片的内部逻辑转换成直接控制电机驱动器的速度和方向信号，驱动直流电机转动；
F)若直流电机转动到位，直流电机停止转动，若直流电机转动不到位，返回E)步，循环动作，直到到位为止。
所述的的控制流程，其所述E)步中，同时，CPLD芯片经H桥控制电路及过流保护电路采集直流电机电流信号，将直流电机电流信号与比较器提供的设定值信号比较，并由光耦隔离电路传送到CPLD芯片中：
A)若采集到的电流值没有超过设定值，驱动直流电机转动；
B)若采集到的电流值超过设定值，则说明直流电机过流或过载，信号马上传送到CPLD芯片，通过CPLD芯片的内部逻辑发出停机命令，使直流电机停止转动。
本发明提出的基于现场总线的双臂巡线机器人控制系统的主要优点如下：采用分布式硬件体系结构，通过现场总线系统将机器人控制主机和各控制单元联系在一起，实现整个控制系统的实时通讯与控制；采用可编程逻辑器件实现对各关节电机的速度与开停控制及过流过载控制，保证系统的实时性与准确性；采用超声、红外、霍尔元件和摄像装置实现对输电线路及线上装置的全面检测，保证机器人自主、准确地在线上行走并跨越障碍；采用系统状态监控单元和检测设备控制单元实现对整个机器人控制系统及检测设备总体运行情况的实时监控监测，保证系统和各检测设备的正常运行，保证系统运行安全，提高了巡检效率。
【附图说明】
图1为本发明基于现场总线的双臂巡线机器人控制系统的结构框图；
图2为本发明基于现场总线的双臂巡线机器人控制系统中运动控制单元原理图；
图3为本发明基于现场总线的双臂巡线机器人控制系统中ARM控制板的电路原理图；
图4为本发明基于现场总线的双臂巡线机器人控制系统中FPGA控制板的电路原理图；
图5为本发明基于现场总线的双臂巡线机器人控制系统中CPLD伺服驱动板的电路原理图；
图6为本发明基于现场总线的双臂巡线机器人控制系统的软件结构图；
图7为本发明基于现场总线的双臂巡线机器人控制系统的控制流程图。
【具体实施方式】
本发明一种基于现场总线的双臂巡线机器人控制系统，是基于嵌入式微处理器和工控机的，适用于高压输电线路巡检作业，由机器人控制主机、运动控制单元、传感器单元、系统状态监测单元、检测设备控制单元组成。
其中，机器人控制主机由嵌入式PC 104计算机、图像采集卡、无线网卡构成，是机器人控制器的核心部分；运动控制单元由ARM控制板、FPGA控制板和CPLD伺服驱动板构成，ARM控制板主要包括ARM9微处理器、现场总线接口、PC104接口和RS232接口，FPFA控制板主要包括FPGA芯片及ROM配置芯片和PC104接口，CPLD伺服驱动板主要包括CPLD芯片及时钟芯片、H桥控制电路及过流保护电路；传感器单元由ARM控制板和各类传感器组成，ARM控制板主要包括ARM9微处理器、现场总线接口，传感器主要有超声传感器、红外传感器、霍尔传感器、触点开关等；系统状态监控单元和检测设备控制单元都由ARM控制板构成，ARM控制板上主要包括ARM9微处理器和现场总线接口。
图1为该机器人控制系统的结构框图。控制系统采用分布式硬件结构，控制主机通过现场总线和各控制单元进行数据传输；控制主机包括PC104主机、图像采集卡和无线网卡等，图像采集卡通过PC104总线与PC104主机连接，无线网卡通过PC104主机上的无线网卡插槽与其连接；运动控制单元包括ARM控制板、FPGA控制板和CPLD伺服驱动板等，FPGA控制板和CPLD伺服驱动板通过PC104插针与ARM控制板连接，FPGA控制板信号通过PWM和DIR接口与CPLD伺服驱动板进行连接；传感器单元包括ARM控制板和各类传感器，各类传感器信号通过A/D转换，连接到ARM控制器的I/O口；系统状态监控单元和检测设备控制单元主要由ARM控制板和相关传感器组成，其传感器信号通过A/D转换连接到ARM控制器的I/O口；运动控制单元、传感器单元、系统状态监控单元和检测设备控制单元通过各自的现场总线接口与控制主机连接，进行实时通信。
其中，PC104、ARM9微处理器、图像采集卡、无线网卡、FPGA和CPLD芯片都是从市场购买的。
图1的工作原理如下：传感器单元中的红外、超声等传感器阵列及摄像装置探测线路损伤及障碍物情况，将传感器信息及图像经过本单元ARM9微处理器进行预处理后(主要是对传感器阵列所探测到的障碍物信息进行汇总并简单处理)，通过现场总线传送给机器人控制主机，作为控制主机进行障碍信息融合与识别、行为层次上的越障规划的依据；运动控制单元通过现场总线接收到控制主机传来的关于关节电机运动速度和方向的信息，首先进入运动控制单元的ARM控制板，经过地址选择器选择需要控制的电机，同时将控制信息通过PC104总线接口发送给相应的FPGA控制板，由FPGA控制板上的程序产生需要的PWM控制信息和电机转向信息，之后PWM信号和转向信号经过CPLD伺服驱动板的组合逻辑传送到H桥控制电路对相关电机进行启停及速度控制；系统状态监控单元实时监测控制主机的工作状态信息，并由单元内的ARM控制板对信息进行预处理，一旦发现计算机死机或重启，ARM控制板立即将信息通过现场总线发送到控制主机，由控制主机向运动控制器发出停止运动命令；检测设备控制单元主要对机器人携带的各种检测设备进行实时监控，并将检测结果通过现场总线传送到控制主机；机器人控制主机汇总各单元信息，完成行为层次上的越障规划、图像采集与传输、障碍信息融合与识别、各单元之间的组织管理等。
图2为基于现场总线的双臂巡线机器人控制系统中运动控制单元结构图。运动控制单元主要包括ARM控制板、FPGA控制板、CPLD伺服驱动板、H桥电路和过流过载保护电路。在单元外，ARM控制板通过现场总线实现和控制主机的通讯，在单元内，ARM控制板通过PC104接口实现与各控制板的通讯：ARM控制板产生的电机控制信号通过PC104总线传送给FPGA控制板，通过FPGA内部程序产生相应电机的速度信号PWM和电机方向信号DIR，PWM和DIR信号通过PWM和DIR接口传送给CPLD伺服驱动板，经过CPLD芯片的内部逻辑产生H桥电路的驱动信号，控制H桥的通断。同时H桥臂上的电流信号经过过流过载保护电路的处理反馈给CPLD伺服驱动板。
图2的工作原理如下：ARM控制板作为运动控制单元的主控板，通过现场总线实现和控制主机的实时通讯，通过PC104接口实现与FPGA控制板的实时通讯，它负责将上位机传来的控制信息译码发送给相应的寄存器，实现对不同电机的准确控制；机器人控制主机通过信息融合及各种现场控制命令，将具体关节电机的运行参数通过现场总线传送给运动控制单元的ARM控制板，ARM控制板将参数进行相应格式化处理通过PC104总线传送给FPGA控制板，并将参数存储在相应寄存器中。在接收到ARM控制板传来的信息后，FPGA控制板根据相应寄存器内容，产生PWM波形，并通过PC104总线传送给CPLD伺服驱动板；CPLD伺服驱动板上的CPLD芯片根据PWM信号通过逻辑运算得到电机转向信号，并产生H桥电路的导通和截止信号，实现对关节电机的速度和方向控制。
图3为本发明的ARM控制板的电路原理图。ARM控制板主要包括ARM9芯片1、FLASH存储器2、时钟电路3、现场总线接口、PC104总线接口和RS232接口。其中，ARM9芯片1分别与时钟电路3、PC104总线5、RS232总线6、现场总线控制器7电连接，现场总线控制器7与现场总线4电连接；并经地址总线、数据总线与FLASH存储器2电连接。
图3的工作原理是通过RS232接口对ARM9芯片1进行编程及通信。时钟电路3为ARM9芯片1提供基频时钟，通过ARM9芯片1内部的倍频模块产生芯片正常工作所需要的时钟频率。系统运行所需要的程序及数据信息存储在FLASH存储器2中。ARM9芯片1通过PC104总线5实现其与机器人控制主机、FPGA控制板、CPLD伺服驱动板之间的通信。通过现场总线4实现各ARM控制板之间的通信，现场总线4由现场总线控制器7掌控。
图4为FPGA控制板的电路原理图。包括现场可编程逻辑器件FPGA8、FPGA配置电路、时钟电路3、模式选择电路12、电源模块13和PC104接口。其中，现场可编程逻辑器件FPGA8分别与时钟电路3、PC104总线5、模式选择电路12、电源模块13、FPGA配置电路电连接，FPGA配置电路包括JTAG电路11和PROM10，其中PROM10和现场可编程逻辑器件FPGA8串行连接在JTAG电路11上；现场可编程逻辑器件FPGA8并经电机速度接口(PWM)和电机方向接口(DIR)与CPLD芯片9电连接。
图4的工作原理是电源模块13为LDO电源，为FPGA控制板的正常工作提供合适的电平。时钟电路3为现场可编程逻辑器件FPGA8提供高频工作时钟。现场可编程逻辑器件FPGA8的配置电路包括JTAG电路11和PROM10，其中PROM10和现场可编程逻辑器件FPGA8串行连接在JTAG电路11上，在配置FPGA时通过模式选择电路12将现场可编程逻辑器件FPGA8的配置模式选为边缘扫描模式，通过JTAG电路11实现对PROM10和现场可编程逻辑器件FPGA8的配置。在断电后，由于现场可编程逻辑器件FPGA8是易失性器件，原先的配置将不复存在，而PROM10是非易失性器件，此时将现场可编程逻辑器件FPGA8的配置模式选择为主串模式，通过PROM10实现对现场可编程逻辑器件FPGA8的配置。从ARM控制板经PC104总线5传来的控制信息达到现场可编程逻辑器件FPGA8后，经过内部时序逻辑和组合逻辑，现场可编程逻辑器件FPGA8产生PWM波形和电机运行方向控制信号DIR，并通过PC104总线5传送给CPLD伺服驱动板。
图5为CPLD伺服驱动板的电路原理图。包括CPLD芯片9、时钟电路3、H桥控制电路14、光耦隔离电路15及过流保护电路。其中，CPLD芯片9分别与时钟电路3、现场可编程逻辑器件FPGA8、JTAG电路11、H桥控制电路14、光耦隔离电路15电连接；H桥控制电路14与直流电机17电连接；光耦隔离电路15与比较器16输出端电连接，比较器16两输入端，一接设定值信号18，另一接驱动器电流19。
图5的工作原理是通过时钟电路3为CPLD芯片9提供可靠的工作时钟。由于CPLD芯片9是非易失性器件，所以只需要通过JTAG电路11对其进行一次性配置即可。从FPGA控制板传来的PWM和DIR信号经过CPLD芯片9的内部逻辑转换成可以直接控制电机驱动器的速度和方向信号，驱动直流电机17转动。同时采集H桥制电路14桥臂上的电流，通过与设定值信号18的电流值进行比较，如果采集到的电流值超过设定值，则说明直流电机17过流或者过载，信号马上传送到CPLD芯片9，通过CPLD芯片9的内部逻辑发出停机命令，达到防止直流电机17过流过载的目的。
本发明的技术核心在于将分布式硬件体系结构应用到机器人运动控制系统中，并通过现场总线技术将各硬件单元有效的联系在一起，实现整个控制系统的实时高效通讯，形成了适应于高压输电线路线上作业的双臂巡线机器人控制系统。
本发明采用双臂式机械结构、分布式硬件结构和现场总线通讯系统，能够实现对机器人各关节的有效实时控制，为高压输电线路上的作业提供了很好的操作平台，通过在巡线机器人本体上加装巡检模块，可以实现对输电线路破损、锈蚀、断股的自动检测；在雨雪冰冻天气时，可以加装除冰模块，清除线路积冰，防止因积冰严重而导致输电线被压断等恶性事故的发生。从而有利于国家电力系统的安全运行，保障人民生活和国家经济的顺利进行。
本发明的基于现场总线的双臂巡线机器人控制系统的软件结构，如图6所示。
本发明的基于现场总线的双臂巡线机器人控制系统的控制流程，如图7所示，包括步骤：
E)开启机器人控制主机，经现场总线4对各单元初始化；
F)检测设备控制单元采集系统、设备信息，传感器单元采集传感器信息，处理后，经现场总线4返回控制主机；
C)控制主机发出运动控制指令，综合各单元返回的信息ARM控制板作动作规划，现场可编程逻辑器件FPGA8产生直流电机17的速度、方向控制信号；
D)直流电机17的速度、方向控制信号传递给CPLD伺服驱动板；
E)经过CPLD芯片9的内部逻辑转换成直接控制电机驱动器的速度和方向信号，驱动直流电机17转动；
F)若直流电机17转动到位，直流电机17停止转动，若直流电机17转动不到位，返回E)步，循环动作，直到到位为止。
在E)步同时，CPLD芯片9经H桥控制电路14及过流保护电路采集直流电机17电流信号，将直流电机17电流信号与比较器16提供的设定值信号18比较，并由光耦隔离电路传送到CPLD芯片中；
A)若采集到的电流值没有超过设定值，驱动直流电机17转动。
B)若采集到的电流值超过设定值，则说明直流电机17过流或过载，信号马上传送到CPLD芯片9，通过CPLD芯片9的内部逻辑发出停机命令，使直流电机17停止转动，达到防止直流电机17过流过载的目的。