大持重液压小吊臂的控制系统及其控制方法
【技术领域】
[0001] 本发明涉及一种机器人技术，尤其是一种高压带电作业机器人大持重液压小吊臂 的控制系统及其控制方法。
【背景技术】
[0002] 为了提高带电作业的自动化水平和安全性，减轻操作人员的劳动强度和强电磁场 对操作人员的人身威胁，从80年代起许多国家都先后开展了带电作业机器人的研究，如日 本、西班牙、美国、加拿大、法国等国家先后开展了对带电作业机器人的研究。2002年我国也 进行了高压带电作业机器人产品化样机的研制。
[0003] 带电作业机器人经过多年的研究与实践，完成了实验室样机的研制，通过大量的 现场试验，验证了机器人在配电网带电作业中的需求与价值，同时也验证了研究方案的可 行性，设计的合理性。但机器人作业内容较为单一，大都是采用双机械臂主从控制方式，不 能涵盖全部配电网带电作业项目，机器人不能在复杂线路环境下开展作业，因此与配电网 带电作业的生产实际存在差距。因此需要研制大持重/自重比小吊臂，为机器人增加辅助 大持重小吊臂，从而完成更换横担、变压器等重载作业任务。
[0004] 大持重液压小吊臂研究一直也是机器人研究的热点之一，目前国际上通常的工业 机器人载荷比在1 :1〇以下。受线路环境及绝缘斗臂车的约束，要求顶部工作台整体重量不 能过大，目前一般在500~600Kg。根据研究经验，辅助小吊臂应限制在lOOKg以内，因此要 求小吊臂应达到3:1左右的高载荷比，并且保证系统具有足够的刚度，以保证末端控制精 度和稳定性。现有的小吊臂结构简单，控制方式大都采用液压阀手动控制，不带有位置传感 器，所以不能进行位置精确控制。
【发明内容】
[0005] 为解决现有技术存在的不足，本发明公开了大持重液压小吊臂的控制系统及其控 制方法，该控制系统采用主从加自主控制方式代替人的手臂完成10kV配电线路更换横担、 更换变压器等重载作业任务。
[0006] 为实现上述目的，本发明的具体方案如下：
[0007] 大持重液压小吊臂的控制系统，包括：遥控器通过光纤收发模块与工控机通信，工 控机连接信号处理模块，信号处理模块输出的模拟量通过伺服放大器与多个液压伺服阀连 接，信号处理模块输出的数字量传送至多个开关阀，遥控器的指令通过光纤收发模块下发 给工控机，工控机通过信号处理模块下发控制指令、开关驱动指令给小吊臂。
[0008] 所述液压伺服阀分别与对应的小吊臂机械臂的第一腰部回转轴、大臂俯仰轴、小 臂俯仰轴、第二腰部回转轴及手爪开合轴相连。第一腰部回转轴、大臂俯仰轴、小臂俯仰轴、 第二腰部回转轴及手爪开合轴分别与对应的模拟电位器相连，模拟电位器均通过传感器集 束系统连接信号处理模块。
[0009] 所述遥控器包括：模拟量采集模块、数字量采集模块分别通过磁藕隔离模块I、磁 藕隔离模块II与微处理器连接，微处理器与磁藕隔离模块III、串口驱动电路、光纤发送模 块依次连接，复位电路、JTAG电路、晶振电路分别与微处理器连接，模拟量采集模块采集主 手电位器的数据，数字量采集模块采集带锁按键、自复位按键的数据，并经过微处理器处理 后，经过光纤收发模块，发送给工控机。
[0010] 所述遥控器带有5自由度模拟主手，用于控制5自由度小吊臂每个关节的运动速 度、位置；遥控器上设置11个按键，分别用于控制液压开关、主/从坐标系切换键、手爪锁 止、小吊臂暂停、一键还原、X轴正方向世界坐标系运动、X轴负方向世界坐标系运动、Y轴正 方向世界坐标系运动、Y轴负方向世界坐标系运动、Z轴正方向世界坐标系运动、Z轴负方向 世界坐标系运动；遥控器上还设置7组状态指示灯，即液压开关、手爪锁止、小吊臂暂停、一 键还原、单轴运动、世界坐标系运动状态指示、安全报警指示。
[0011] 所述模拟量采集模块采集5自由度模拟主手信号，电压范围0~10V，数字量采集 模块采集11个按键的开关信号，高电平5V、低电平0V，数字量输出模块输出开关信号控制 7组状态指示灯。
[0012] 所述磁藕隔离模块I、磁藕隔离模块II与磁藕隔离模块III均为四通道数字隔离 器，两端工作电压2. 7V~5. 5V，支持低电压工作并能实现电平转换。
[0013] 所述信号处理模块输出5路模拟量信号，电流范围0~20ma ;输出3路开关量信 号。
[0014] 所述小吊臂是5自由度液压机械臂；角度传感器设于小吊臂上，角度传感器测量 机械臂每个轴的角度大小，机械臂机械结构包括基座，基座上安装有大臂，大臂可以实现腰 部回转、大臂俯仰，上端为肩关节，小臂安装在肩关节上，小臂可以实现腕部回转，小臂的前 端与手爪连接。
[0015] 大持重液压小吊臂主手控制系统控制时分为主端和从端两部分，主端发送命令给 从端，从端反馈信息给主端，操作主手运动，工控机实时采集小吊臂模拟电位器的位置信息 并将位置信息发送给主手控制器，工控机通过对主手的位置跟踪来完成机械臂的运动。 [0016] 本发明还公开了大持重液压小吊臂的控制方法，包括如下步骤：
[0017] 1)对每个杆件在关节轴处建立一个笛卡儿坐标系（Xl，yi， Zl)，i是1到5之间的所 有正整数，5为自由度数目，再加上基座坐标系（X(]，y<]， Z。）；
[0018] 2)为每个关节处的杆件坐标系建立4X4奇次变换矩阵，表示与前一个杆件即当 前杆件的前一个杆件的坐标系的关系；
[0019] 3)采用"边算边走"的定时插补算法，计算插补点的位置和姿态；"边算边走"是指 将各插补点进行逆运动学变换后得到的关节位置不用存储，而直接再按这些关节位置开始 运动；
[0020] 4)计算每个轴的运动学反解，运动学反解是指已知末端位置和姿态求每个关节的 角度，得出插补周期内的每个轴的运动角度。
[0021] 所述步骤中1)中确定和建立每个坐标系采用下面三条规则：每个关节i的运动都 绕着Zi轴运动；x i轴垂直z i i轴并指向离开z i i轴的方向；y ^由按右手坐标系得要求建立。
[0022] 本发明的有益效果：
[0023] 1.本发明自行设计的大持重液压小吊臂控制系统经实验验证，代替人的手臂完成 10kV配电带电更换横担、变压器重载任务。
[0024] 2.小吊臂控制系统可以实现一键还原功能，可以使小吊臂很快恢复初始状态。
[0025] 3.控制系统可以实现主从和自主运动双重功能，功能强大、操作方便。
[0026] 4.主控制器采用主工控机，主工控机实现运动学正反解、插补算法，可以实现复杂 的运动控制，处理速度快。
[0027] 5.控制系统带有安全报警功能，超过额定载荷时自动停止并报警。
[0028] 6.遥控器与机械臂控制部分，通过光纤进行电气隔离，保证了人员的安全。
【附图说明】
[0029] 图1是本发明结构原理总框图；
[0030] 图2是本发明遥控器原理图；
[0031] 图3是本发明机械臂结构图；
[0032] 图4是本发明一键还原原理图；
[0033] 图5是本发明主手控制流程图；
[0034] 其中，1、遥控器；2、光纤发送模块；3、光纤接收模块；4、工控机；5、信号处理模块； 6、模拟电位器；7、传感器集束系统；8、伺服阀；9、开关阀；10、模拟量米集模块；11、数字量 采集模块；12、磁藕隔离模块I ;13、磁藕隔离模块II ;14、磁藕隔离模块III ;15、复位电路； 16、JTAG电路；17、微处理器；18、串口驱动电路；19、晶振电路。
[0035] JTAG的全称是Joint Test Action Group，即联合测试行动小组。目前，JTAG已 成为一种国际标准测试协议，主要用于各类芯片的内部试。
【具体实施方式】：
[0036] 下面结合附图对本发明进行详细说明：
[0037] 参见图1，一种大持重液压小吊臂控制系统，包括遥控器1、光纤发送模块2、光纤 接收模块3、工控机4、信号处理模块5、模拟电位器6、传感器集束系统7、伺服阀8、开关阀 9〇
[0038] 遥控器1控制面板的输出接数据采集板的输入，数据采集板的输出接光纤发送模 块2 ;光纤接收模块3的输出通过串口接工控机4的输入，工控机4的输出通过串口接信号 处理模