CN105234942B - 大持重液压小吊臂的控制***及其控制方法 - Google Patents

Abstract

本发明公开了大持重液压小吊臂的控制***及其控制方法，遥控器通过光纤收发模块与工控机通信，工控机连接信号处理模块，信号处理模块输出的模拟量通过伺服放大器与多个液压伺服阀连接，角度传感器设于小吊臂上，且通过传感器集束***连接信号处理模块，遥控器的指令通过光纤收发模块下发给工控机，工控机通过信号处理模块下发控制指令、开关驱动指令给小吊臂。本发明自行设计的大持重液压小吊臂控制***经实验验证，代替人的手臂完成10kV配电带电更换横担、变压器重载任务。

Description

大持重液压小吊臂的控制***及其控制方法

技术领域

本发明涉及一种机器人技术，尤其是一种高压带电作业机器人大持重液压小吊臂的控制***及其控制方法。

背景技术

为了提高带电作业的自动化水平和安全性，减轻操作人员的劳动强度和强电磁场对操作人员的人身威胁，从80年代起许多国家都先后开展了带电作业机器人的研究，如日本、西班牙、美国、加拿大、法国等国家先后开展了对带电作业机器人的研究。2002年我国也进行了高压带电作业机器人产品化样机的研制。

带电作业机器人经过多年的研究与实践，完成了实验室样机的研制，通过大量的现场试验，验证了机器人在配电网带电作业中的需求与价值，同时也验证了研究方案的可行性，设计的合理性。但机器人作业内容较为单一，大都是采用双机械臂主从控制方式，不能涵盖全部配电网带电作业项目，机器人不能在复杂线路环境下开展作业，因此与配电网带电作业的生产实际存在差距。因此需要研制大持重/自重比小吊臂，为机器人增加辅助大持重小吊臂，从而完成更换横担、变压器等重载作业任务。

大持重液压小吊臂研究一直也是机器人研究的热点之一，目前国际上通常的工业机器人载荷比在1：10以下。受线路环境及绝缘斗臂车的约束，要求顶部工作台整体重量不能过大，目前一般在500～600Kg。根据研究经验，辅助小吊臂应限制在100Kg以内，因此要求小吊臂应达到3:1左右的高载荷比，并且保证***具有足够的刚度，以保证末端控制精度和稳定性。现有的小吊臂结构简单，控制方式大都采用液压阀手动控制，不带有位置传感器，所以不能进行位置精确控制。

发明内容

为解决现有技术存在的不足，本发明公开了大持重液压小吊臂的控制***及其控制方法，该控制***采用主从加自主控制方式代替人的手臂完成10kV配电线路更换横担、更换变压器等重载作业任务。

为实现上述目的，本发明的具体方案如下：

大持重液压小吊臂的控制***，包括：遥控器通过光纤收发模块与工控机通信，工控机连接信号处理模块，信号处理模块输出的模拟量通过伺服放大器与多个液压伺服阀连接，信号处理模块输出的数字量传送至多个开关阀，遥控器的指令通过光纤收发模块下发给工控机，工控机通过信号处理模块下发控制指令、开关驱动指令给小吊臂。

所述液压伺服阀分别与对应的小吊臂机械臂的第一腰部回转轴、大臂俯仰轴、小臂俯仰轴、第二腰部回转轴及手爪开合轴相连。第一腰部回转轴、大臂俯仰轴、小臂俯仰轴、第二腰部回转轴及手爪开合轴分别与对应的模拟电位器相连，模拟电位器均通过传感器集束***连接信号处理模块。

所述遥控器包括：模拟量采集模块、数字量采集模块分别通过磁藕隔离模块I、磁藕隔离模块II与微处理器连接，微处理器与磁藕隔离模块III、串口驱动电路、光纤发送模块依次连接，复位电路、JTAG电路、晶振电路分别与微处理器连接，模拟量采集模块采集主手电位器的数据，数字量采集模块采集带锁按键、自复位按键的数据，并经过微处理器处理后，经过光纤收发模块，发送给工控机。

所述遥控器带有5自由度模拟主手，用于控制5自由度小吊臂每个关节的运动速度、位置；遥控器上设置11个按键，分别用于控制液压开关、主/从坐标系切换键、手爪锁止、小吊臂暂停、一键还原、X轴正方向世界坐标系运动、X轴负方向世界坐标系运动、Y轴正方向世界坐标系运动、Y轴负方向世界坐标系运动、Z轴正方向世界坐标系运动、Z轴负方向世界坐标系运动；遥控器上还设置7组状态指示灯，即液压开关、手爪锁止、小吊臂暂停、一键还原、单轴运动、世界坐标系运动状态指示、安全报警指示。

所述模拟量采集模块采集5自由度模拟主手信号，电压范围0～10V，数字量采集模块采集11个按键的开关信号，高电平5V、低电平0V，数字量输出模块输出开关信号控制7组状态指示灯。

所述磁藕隔离模块I、磁藕隔离模块II与磁藕隔离模块III均为四通道数字隔离器，两端工作电压2.7V～5.5V，支持低电压工作并能实现电平转换。

所述信号处理模块输出5路模拟量信号，电流范围0～20ma；输出3路开关量信号。

所述小吊臂是5自由度液压机械臂；角度传感器设于小吊臂上，角度传感器测量机械臂每个轴的角度大小，机械臂机械结构包括基座，基座上安装有大臂，大臂可以实现腰部回转、大臂俯仰，上端为肩关节，小臂安装在肩关节上，小臂可以实现腕部回转，小臂的前端与手爪连接。

大持重液压小吊臂主手控制***控制时分为主端和从端两部分，主端发送命令给从端，从端反馈信息给主端，操作主手运动，工控机实时采集小吊臂模拟电位器的位置信息并将位置信息发送给主手控制器，工控机通过对主手的位置跟踪来完成机械臂的运动。

本发明还公开了大持重液压小吊臂的控制方法，包括如下步骤：

1)对每个杆件在关节轴处建立一个笛卡儿坐标系(x_i,y_i,z_i)，i是1到5之间的所有正整数，5为自由度数目，再加上基座坐标系(x₀,y₀,z₀)；

2)为每个关节处的杆件坐标系建立4×4奇次变换矩阵，表示与前一个杆件即当前杆件的前一个杆件的坐标系的关系；

3)采用“边算边走”的定时插补算法，计算插补点的位置和姿态；“边算边走”是指将各插补点进行逆运动学变换后得到的关节位置不用存储，而直接再按这些关节位置开始运动；

4)计算每个轴的运动学反解，运动学反解是指已知末端位置和姿态求每个关节的角度，得出插补周期内的每个轴的运动角度。

所述步骤中1)中确定和建立每个坐标系采用下面三条规则：每个关节i的运动都绕着z_i轴运动；x_i轴垂直z_i-1轴并指向离开z_i-1轴的方向；y_i轴按右手坐标系得要求建立。

本发明的有益效果：

1.本发明自行设计的大持重液压小吊臂控制***经实验验证，代替人的手臂完成10kV配电带电更换横担、变压器重载任务。

2.小吊臂控制***可以实现一键还原功能，可以使小吊臂很快恢复初始状态。

3.控制***可以实现主从和自主运动双重功能，功能强大、操作方便。

4.主控制器采用主工控机，主工控机实现运动学正反解、插补算法，可以实现复杂的运动控制，处理速度快。

5.控制***带有安全报警功能，超过额定载荷时自动停止并报警。

6.遥控器与机械臂控制部分，通过光纤进行电气隔离，保证了人员的安全。

附图说明

图1是本发明结构原理总框图；

图2是本发明遥控器原理图；

图3是本发明机械臂结构图；

图4是本发明一键还原原理图；

图5是本发明主手控制流程图；

其中，1、遥控器；2、光纤发送模块；3、光纤接收模块；4、工控机；5、信号处理模块；6、模拟电位器；7、传感器集束***；8、伺服阀；9、开关阀；10、模拟量采集模块；11、数字量采集模块；12、磁藕隔离模块I；13、磁藕隔离模块II；14、磁藕隔离模块III；15、复位电路；16、JTAG电路；17、微处理器；18、串口驱动电路；19、晶振电路。

JTAG的全称是Joint Test Action Group，即联合测试行动小组。目前，JTAG已成为一种国际标准测试协议，主要用于各类芯片的内部试。

具体实施方式：

下面结合附图对本发明进行详细说明：

参见图1，一种大持重液压小吊臂控制***，包括遥控器1、光纤发送模块2、光纤接收模块3、工控机4、信号处理模块5、模拟电位器6、传感器集束***7、伺服阀8、开关阀9。

遥控器1控制面板的输出接数据采集板的输入，数据采集板的输出接光纤发送模块2；光纤接收模块3的输出通过串口接工控机4的输入，工控机4的输出通过串口接信号处理模块5的输入，信号处理模块5的输出接伺服阀8和开关阀9；模拟电位器6通过传感器集束***7接工控机4的输入。所述遥控器1的指令，通过光纤发送模块2和光纤接收模块3下发给工控机4，工控机4通过信号处理模块5下发速度指令、开关驱动指令给液压***。

参见图2，所述遥控器1采集板包括模拟量采集模块10、数字量采集模块11、磁藕隔离模块I12、磁藕隔离模块II13、磁藕隔离模块III14、复位电路15、JTAG电路16、微处理器17、串口驱动电路18、晶振电路19。模拟量采集模块10采集7路模拟摇杆的数据，数字量采集模块11采集带锁按键、自复位按键的数据，并经过微处理器17处理后，经过串口驱动电路18、无线发送模块2，发送给无线接收模块3。

参见图3，所述机械臂为5自由度，机械结构包括基座，基座上安装有大臂，大臂可以实现腰部回转、大臂俯仰，上端为肩关节，小臂安装在肩关节上，小臂可以实现腕部回转，小臂的前端与手爪连接。

参见图4，大持重液压小吊臂处于自动运动方式时，按照事先编写好的运动控制程序自动循环执行，直到按下控制面板上的暂停按键。

复位操作例程，复位按钮是否按下，如果没有则例程结束，如果按下则继续判断Start按钮是否按下，如果没有则等待，如果按下则判断复位符号是否为零，如果为零，则启动复位运动，检测复位标志开关，直至复位完毕，结束，复位符号如果不为零，则启动归零运动，到位检测例程，直至复位完毕。

参见图5，大持重液压小吊臂主手控制***分为主端和从端两部分。主端发送命令给从端，从端反馈信息给主端，操作者操作主手运动，小吊臂控制器实时采集小吊臂电位器的位置信息并将位置信息发送给主手控制器，机械臂控制器通过对主手的位置跟踪来完成机械臂的运动，同时主手控制器再将该位置信息发送给手持终端，供显示。主手控制器是小吊臂控制***的一部分，主手控制***是遥控器的控制***。

初始化控制器，主手/坐标系切换按键，然后判断是否主手控制，如果是开始机械臂位值检测及主手位置检测，机械臂位值检测时向主手控制器发送位值信息，然后将主手、机械臂位值信息相减，然后判断参数B>e>0，如果满足，则主手向正方向运动，如果-B<e<0，则主手向反方向运动。

本申请中，微处理器采用32位TMS320F2812DSP芯片，主频可达150MHz。

磁藕隔离模块选用ADI公司的ADuM1400/1/2四通道数字隔离器。两端工作电压2.7V～5.5V，支持低电压工作并能实现电平转换。

所述模拟量采集模块采集5自由度模拟主手信号，电压范围0～10V，数字量采集模块采集11个按键的开关信号，高电平5V、低电平0V，数字量输出模块输出开关信号控制7组状态指示灯。

5自由度模拟主手具备单关节控制、单关节位置反馈及安全监控等功能。

工控机采用ARK-5260小型工控机，主频1.66GHz，支持4路485接口，自带2路PCI扩展槽，12-24V直流电源供电。

信号处理模块输出5路模拟量信号，电流范围0～20ma；输出1路开关量信号，驱动电流10A，12V；输入5路模拟量信号，电压范围0～10V。

角度传感器采用WX13-12-15K单圈精密电位器,允许偏差0.05,标称阻值0～15kΩ。

液压伺服阀采用中船重工CSDY1射流管伺服阀，供电电流0～20ma。

光纤通信模块包括光纤发送模块2、光纤接收模块3，用于遥控器和工控机之间的通信，采用工业级RS485总线光纤中继器，光纤类型：单模、多模可选，光口连接器：标配ST，光纤采用单模光纤，波长1310nm，传输距离0～20Km。

本发明还公开了一种大持重小吊臂自主控制方法，包括如下步骤：

1)对每个杆件在关节轴处建立一个笛卡儿坐标系(x_i,y_i,z_i)，i是1到5之间的所有正整数，5为自由度数目，再加上基座坐标系(x₀,y₀,z₀)(在机座上的位置和方向可任选，只要z₀轴沿着第一关节运动轴即可)；

2)为每个关节处的杆件坐标系建立4×4奇次变换矩阵，表示与前一个杆件(当前杆件的前一个杆件)坐标系的关系；

3)采用“边算边走”的定时插补算法，计算插补点的位置和姿态；“边算边走”是指将各插补点进行逆运动学变换后得到的关节位置不用存储，而直接再按这些关节位置开始运动；

4)采用公式法计算每个轴的运动学反解(运动学反解是指已知末端位置和姿态求每个关节的角度)，得出插补周期内的每个轴的运动角度；

所述步骤中1)确定和建立每个坐标系采用下面三条规则：每个关节i(i是1到5之间的所有正整数，5为自由度数目)的运动都绕着z_i轴运动；x_i轴垂直z_i-1轴并指向离开z_i-1轴的方向；y_i轴按右手坐标系得要求建立。

该发明空间直线插补可分为以下几步完成:

输入机器人运动的初始点P₀(x₀,y₀,z₀)和终点P_f(x_f,y_f,z_f)(f是final的缩写)运动速度P_v，加减速时间T_a和插补周期T_c，运行时间T；

基本参数的确定和插补点的求解方法。由于机器人空间直线运动需经过加减速和匀速运动段，因此在进行插补运动前，应确定P_v是否满足加减速要求。方法如下：

由P₀(x₀,y₀,z₀)和P_f(x_f,y_f,z_f)得到实际运动距离P_d＝|P₀P_f|；由P_v和T_a可计算出加减速段所需距离若C_d≥P_d，则实际运动速度否则C_v＝P_v；由时间T_a和插补时间T_c得出加速步数S_a。由P₀(x₀,y₀,z₀)和P_f(x_f,y_f,z_f),可得空间直线参数方程

因此由式(1)，可得各插补点P_i(x_i,y_i,z_i)(i为各插补点的步号，在0与之间的所有正整数)到P₀的距离为

由式(3)就可求出k，并得到插补点直角坐标。因此空间直线插补算法关键在于确定各插补段运动距离S_d(i)。下面介绍运动各段求取S_d(i)方法：

加速运动段。由于本文设计的机器人加速段为匀加速运动，故由实际运动速度C_v和加减速时间T_a求得加速度(单位是m/s^2)，因此加速度段上第i个插补点的速度S_cv(i)＝iT_c·a，可得到

匀速运动段。由于本文设计的机器人要求必须经过减速段，而且插补运算为“边算边走”，故每次进行匀速运动段开始前，必须计算所剩距离能否满足***减速要求。匀速段各插补段运动距离S_d(i)＝C_v·T_c

减速运动段。由于在求取加速步数S_a时进行取整计算，因此不能简单的将加速段加速度取反后规划加速段，这样会引入误差，故减速段加速度应重新计算。经过前面i-1个插补点后，可得所剩距离L_d(i)＝P_d-C_Sd(i-1)，因此可得减速段加速度则减速度段上第m个插补点的速度S_cv(m)＝C_v+mT_c·a，即可得到

该发明采用公式法进行运动学反解(运动学反解是指已知末端位置和姿态求每个关节的角度：

θ₁，...，θ₆------表示每个轴运动的角度；

A_i∈R^4×4(i＝1,2,…,6)------是根据D-H坐标系建立的各连杆上坐标系间的转换矩阵。

s_i-----表示sinθ_i；

c_i------表示cosθ_i；

s_ij------表示sin(θ_i+θ_j)；

c_ij-------表示cos(θ_i+θ_j)

n—手的法向矢量

s—手的滑动矢量

a—手的接近矢量

p—手的位置矢量 (6)

坐标系O_i(i是正整数i＝0,1,......,6)则是在操作臂上建立的D-H坐标系；a₂,d₄∈R分别表示机械臂对应连杆的长度。可以将操作臂末端坐标系O₆在基座坐标系O₀下的位姿写成如下表达式：⁰T₆＝A₁A₂A₃A₄A₅A₆。求解运动方程时，从⁰T₆开始求解关节位置。使⁰T₆的符号表达式的各元素等于⁰T₆的一般形式，并据此确定θ₁。一旦求得θ₁之后，可由A₁ ^-1左乘⁰T₆的一般形式，得

A^-1 ₁ ⁰T₆＝¹T₆ (7)

此式可用来求解其他各关节变量。不断地用A的逆矩阵左乘(7)，可得下列另四个矩阵方程式：

A₂ ^-1A₁ ^{-1 0}T₆＝²T₆ (8)

A₃ ^-1A₂ ^-1A₁ ^{-1 0}T₆＝³T₆ (9)

A₄ ^-1A₃ ^-1A₂ ^-1A₁ ^{-1 0}T₆＝⁴T₆ (10)

A₅ ^-1A₄ ^-1A₃ ^-1A₂ ^-1A₁ ^{-1 0}T₆＝⁵T₆ (11)

上式各方程的左式为⁰T₆和前(i-1)个关节变量的函数，可用这些方程来确定各关节的位置：

θ₁＝a tan2(p_y,p_x) (-3.1415≤θ₁≤3.1415) (12)

注意：式中，正、负号对应θ₃的两种可能解。

θ₂＝a tan2(s₂₃,c₂₃)-θ₃(-1.5707≤θ₁≤1.5707) (14)

θ₄＝a tan2(-a_xs₁+a_yc₁,-a_xc₁c₂₃-a_ys₁c₂₃+a_zs₂₃)(-3.1415≤θ₁≤3.1415) (15)

注意：当s₅＝0时，机械臂处于奇异形位。此时，关节轴4和6重合，只能解出θ₄与θ₆的和或差。奇异形位可以由式(15)中atan2的两个变量是否都接近零来判断。

θ₅＝a tan2(-a_x(c₁c₂₃c₄+s₁s₄)-a_y(s₁c₂₃c₄-c₁s₄)+a_zs₂₃c₄,-a_xc₁s₂₃-a_ys₂₃s₁-a_zc₂₃)

(-3.9268≤θ₁≤0.7853) (16)

k₁＝-n_x(c₁c₂₃s₄-s₁c₄)-n_y(s₁c₂₃s₄+c₁c₄)+n_zs₂₃s₄

k₂＝n_x((c₁c₂₃c₄+s₁s₄)c₅-c₁s₂₃s₅)+n_y((s₁c₂₃c₄-c₁s₄)c₅-s₁s₂₃s₅)-n_z(s₂₃c₄c₅+c₂₃s₅)

θ₆＝a tan2(k₁,k₂)(-3.1415≤θ₁≤3.1415) (17)

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (6)

1.大持重液压小吊臂的控制***，其特征是，包括：遥控器通过光纤收发模块与工控机通信，工控机连接信号处理模块，信号处理模块输出的模拟量通过伺服放大器与多个液压伺服阀连接，信号处理模块输出的数字量传送至多个开关阀，遥控器的指令通过光纤收发模块下发给工控机，工控机通过信号处理模块下发控制指令、开关驱动指令给小吊臂；

所述小吊臂是5自由度液压机械臂；角度传感器设于小吊臂上，角度传感器测量机械臂每个轴的角度大小，机械臂机械结构包括基座，基座上安装有大臂，大臂可以实现腰部回转、大臂俯仰，上端为肩关节，小臂安装在肩关节上，小臂可以实现腕部回转，小臂的前端与手爪连接；

所述液压伺服阀分别与对应的小吊臂机械臂的第一腰部回转轴、大臂俯仰轴、小臂俯仰轴、第二腰部回转轴及手爪开合轴相连；第一腰部回转轴、大臂俯仰轴、小臂俯仰轴、第二腰部回转轴及手爪开合轴分别与对应的模拟电位器相连，模拟电位器均通过传感器集束***连接信号处理模块；

所述遥控器带有5自由度模拟主手，用于控制5自由度小吊臂每个关节的运动速度、位置；遥控器上设置11个按键，分别用于控制液压开关、主/从坐标系切换键、手爪锁止、小吊臂暂停、一键还原、X轴正方向世界坐标系运动、X轴负方向世界坐标系运动、Y轴正方向世界坐标系运动、Y轴负方向世界坐标系运动、Z轴正方向世界坐标系运动、Z轴负方向世界坐标系运动；遥控器上还设置7组状态指示灯，即液压开关、手爪锁止、小吊臂暂停、一键还原、单轴运动、世界坐标系运动状态指示、安全报警指示；

大持重液压小吊臂处于自动运动方式时，按照事先编写好的运动控制程序自动循环执行，直到按下控制面板上的暂停按键；

大持重液压小吊臂主手控制***控制时分为主端和从端两部分，主端发送命令给从端，从端反馈信息给主端，操作主手运动，小吊臂控制器实时采集小吊臂电位器的位置信息并将位置信息发送给主手控制器，机械臂控制器通过对主手的位置跟踪来完成机械臂的运动，同时主手控制器再将该位置信息发送给手持终端，供显示；

主手控制时，初始化控制器，主手/坐标系切换按键，然后判断是否主手控制，如果是开始机械臂位值检测及主手位置检测，机械臂位值检测时向主手控制器发送位值信息，然后将主手、机械臂位置信息相减，然后根据判断参数确定主手向正方向运动或主手向反方向运动；

复位操作例程，复位按钮是否按下，如果没有则例程结束，如果按下则继续判断Start按钮是否按下，如果没有则等待，如果按下则判断复位符号是否为零，如果为零，则启动复位运动，检测复位标志开关，直至复位完毕，结束，复位符号如果不为零，则启动归零运动，到位检测例程，直至复位完毕；

该控制***采用主从加自主控制方式代替人的手臂完成10kV配电线路更换横担、更换变压器重载作业任务。

2.如权利要求1所述的大持重液压小吊臂的控制***，其特征是，所述遥控器包括：模拟量采集模块、数字量采集模块分别通过磁藕隔离模块I、磁藕隔离模块II与微处理器连接，微处理器与磁藕隔离模块III、串口驱动电路、光纤发送模块依次连接，复位电路、JTAG电路、晶振电路分别与微处理器连接，模拟量采集模块采集主手电位器的数据，数字量采集模块采集带锁按键、自复位按键的数据，并经过微处理器处理后，经过光纤收发模块，发送给工控机。

3.如权利要求1所述的大持重液压小吊臂的控制***，其特征是，所述模拟量采集模块采集5自由度模拟主手信号，电压范围0～10V，数字量采集模块采集11个按键的开关信号，高电平5V、低电平0V，数字量输出模块输出开关信号控制7组状态指示灯。

4.如权利要求2所述的大持重液压小吊臂的控制***，其特征是，所述磁藕隔离模块I、磁藕隔离模块II与磁藕隔离模块III均为四通道数字隔离器，两端工作电压2.7V～5.5V，支持低电压工作并能实现电平转换；

所述信号处理模块输出5路模拟量信号，电流范围0～20ma；输出1路开关量信号。

5.大持重液压小吊臂的控制方法，其特征是，包括如下步骤：

1)对每个杆件在关节轴处建立一个笛卡儿坐标系(x_i,y_i,z_i)，i是1到5之间的所有正整数，5为自由度数目，再加上基座坐标系(x₀,y₀,z₀)；

2)为每个关节处的杆件坐标系建立4×4奇次变换矩阵，表示与前一个杆件即当前杆件的前一个杆件的坐标系的关系；

3)采用“边算边走”的定时插补算法，计算插补点的位置和姿态；“边算边走”是指将各插补点进行逆运动学变换后得到的关节位置不用存储，而直接再按这些关节位置开始运动；

4)计算每个轴的运动学反解，运动学反解是指已知末端位置和姿态求每个关节的角度，得出插补周期内的每个轴的运动角度。

6.如权利要求5所述的大持重液压小吊臂的控制方法，其特征是，所述步骤中1)中确定和建立每个坐标系采用下面三条规则：每个关节i的运动都绕着z_i轴运动；x_i轴垂直z_i-1轴并指向离开z_i-1轴的方向；y_i轴按右手坐标系得要求建立。