CN106476012B - 一种辅助机械臂控制***的控制方法 - Google Patents

Abstract

本发明公开了一种新型辅助机械臂控制***及其控制方法，包括主操纵杆和主控制器,所述的主控制器的输入端连接模拟量采集模块和数字量采集模块，其输出端设有模拟量输出模块，所述的主操纵杆与模拟量采集模块、数字量采集模块相连，所述的模拟量输出模块输出的模拟量通过伺服放大器放大后传送给多个液压伺服阀，所述的多个液压伺服阀控制辅助机械臂运动,且辅助机械臂的回转油缸上的限位传感器采集的信号反馈给主控制器,辅助机械臂的摆动油缸上的直线位移传感器采集信号反馈给主控制器，主控制其再根据反馈信号对辅助机械臂进行精确控制。

Description

一种辅助机械臂控制***的控制方法

技术领域

本发明涉及配网带电作业领域，具体的说，是一种涉及高压带电作业机器人的辅助机械臂控制器及其控制方法，主要用于与机械臂配合使用，完成配电带电作业中的更换横担、变压器等重载作业任务。

背景技术

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

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

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

发明内容

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

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

大持重液压辅助机械臂的控制***，包括主操纵杆和主控制器,所述的主控制器的输入端连接模拟量采集模块和数字量采集模块，其输出端设有模拟量输出模块，所述的主操纵杆与模拟量采集模块、数字量采集模块相连，所述的模拟量输出模块输出的模拟量通过伺服放大器放大后传送给多个液压伺服阀，所述的多个液压伺服阀控制辅助机械臂运动,且辅助机械臂的回转油缸上的限位传感器采集的信号反馈给主控制器,辅助机械臂的摆动油缸上的直线位移传感器采集信号反馈给主控制器，主控制器再根据反馈信号对辅助机械臂进行精确控制。

进一步的，所述的辅助机械臂，包括底座、大臂、前臂和手爪，在所述的底座上设有一个液压摆动油缸，液压摆动油缸驱动大臂底座在水平面内180度旋转，所述的大臂底座与大臂通过关节II相连，且大臂底座上铰接一个驱动大臂做俯仰运动的俯仰油缸I，所述的大臂的末端通过关节III铰接与其垂直的前臂，所述的前臂在俯仰油缸II的驱动下做俯仰运动；在前臂的末端通过关节IV连接一个手爪。

进一步的，所述主操纵杆有X、Y两个运动自由度，能同时进行X、Y方向的运动控制信号输出；主操纵杆X自由度控制辅助机械臂的关节II；主操纵杆Y自由度控制辅助机械臂的关节III，从而确保机械臂末端的水平前后与垂直上下运动。

进一步的，所述液压摆动油缸上端与大臂底座连接，所述的摆动油缸能在水平面内0～180 度范围内旋转。

进一步的，所述大臂底座一端与液压摆动油缸连接，另一端与大臂相连，且俯仰油缸I 底座固定在大臂底座上；俯仰油缸I的活塞杆驱动大臂做俯仰运动。

进一步的，所述俯仰油缸I带有直线位移传感器I，其通过伺服阀精确控制大臂的俯仰运动。

进一步的，所述俯仰油缸II带有直线位移传感器II，其通过伺服阀精确控制前臂的俯仰运动。

利用所述的控制***对辅助机械臂的大臂、前臂的摆动角度的控制方法，如下：

主操纵杆的模拟量数据输出作为控制指令输入给主控制器；主控制器控制机械臂运动；机械臂的运动位移通过直线位移传感器反馈给主控制器，直线位移传感器I、直线位移传感器II的采集的数据进入主控制器后经过姿态解析算法计算出伺服阀驱动数据分别驱动伺服阀I、伺服阀II使关节II、关节III运动到角度θ₁、θ₂，主控制器通过控制伺服阀的流量来控制液压执行器的动作位移，最终实现对机械臂的位置伺服控制。

所述的姿态解析算法具体如下：

设大臂长度为a₁、前臂长度为a₂，关节II、关节III的关节轴平行，关节II的旋转角度为θ₁；关节III的旋转角度为θ₂；以基座为坐标系为{A}；关节III所在的坐标为坐标系为{B}；先求解机械臂末端在基座坐标系{A}中的位置，然后再求解θ₁、θ₂的大小。

具体如下：

步骤(1)设机械臂末端在坐标系{A}中的位置为

步骤(2)由已知条件求得机械臂末端在坐标系{B}中的位置为

步骤(3)为关节II、关节III的大臂底座、大臂、前臂坐标系建立3×3奇次变换矩阵表示与前一个部件坐标系的关系；由已知条件可知其中：

步骤(4)由步骤(1)、(2)、(3)可得：

步骤(5)求得：

步骤(6)求得机械臂末端在坐标系{A}中的位置根据机械臂杆长a₁、a₂和机械臂末端在坐标系{A}中的位置，即可求出相应的θ₁和θ₂，解得：

上述的 ^Ap_B均表示中间参数。

本发明的有益效果：

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

2.辅助机械臂控制***可以实现大臂俯仰和前臂俯仰两轴联动，末端可以进行世界坐标系运动。

3.控制***设计了一种新型的控制算法，算法实现简单，避免采用复杂的运动学正反解算法。

4.本***主控制器采用成熟的PLC控制器，其抗干扰能力强，故障率低，并且兼容C语言编程，可以进行复杂的浮点运算功能，如：算术运算、逻辑运算和三角函数。

附图说明

图1是本发明辅助机械臂结构图；

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

图3是本发明平面两自由度串联机械臂结构图；

图4是本发明辅助机械臂控制流程图；

图5(1)、图5(2)、图5(3)、图5(4)是本发明主操纵杆示意图；

图中:1底座，2摆动油缸，3大臂底座，4大臂油缸，5大臂，6前臂油缸，7前臂，8 手爪，9关节I，10关节II，11关节III，12关节IV。

具体实施方式：

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

本发明所控制的机械臂包括底座、大臂、前臂和手爪，在所述的底座上设有一个液压摆动油缸，液压摆动油缸驱动大臂底座在水平面内180度旋转，所述的大臂底座与大臂通过关节II10相连，且大臂底座上铰接一个驱动大臂做俯仰运动的俯仰油缸I，所述的大臂的末端通过关节III11铰接与其垂直的前臂，所述的前臂在俯仰油缸II的驱动下做俯仰运动；在前臂的末端通过关节IV12连接一个手爪。

具体的参见图1，本发明的控制***针对的带电作业用辅助机械臂，包括底座1，摆动油缸2，大臂底座3，大臂油缸4，大臂5，前臂油缸6，前臂7，手爪8。

摆动油缸2通过螺钉连接到底座1上，大臂底座3通过螺钉一端连接和摆动油缸2连接在一起，另一端连接和大臂5相连接，大臂油缸4通过螺钉一端连接到大臂油缸座3，另一端连接在大臂5上，前臂7通过螺钉和大臂5连接在一起，前臂油缸6通过螺钉一端连接在大臂5上，另一端连接在前臂7上，手爪8通过螺钉连接在前臂7上。其中关节I9指的是摆动油缸2与大臂底座3的旋转关节；所述的大臂底座与大臂通过关节II10相连；所述的大臂的末端通过关节III11铰接与其垂直的前臂；前臂的末端通过关节IV12连接一个手爪。

进一步的，液压摆动油缸上端与大臂底座连接，所述的摆动油缸能在水平面内0～180度范围内旋转。

进一步的，大臂底座一端与液压摆动油缸连接，另一端与大臂相连，且俯仰油缸I底座固定在大臂底座上；俯仰油缸I的活塞杆驱动大臂做俯仰运动。

进一步的，俯仰油缸I带有直线位移传感器I，其通过伺服阀精确控制大臂的俯仰运动。

进一步的，俯仰油缸II带有直线位移传感器II，其通过伺服阀精确控制前臂的俯仰运动。

针对上面所述的机械臂，其控制***如图2所示，本发明中辅助机械臂控制***，包括主操纵杆、PLC主控制器、回转油缸伺服阀、大臂油缸伺服阀、前臂油缸伺服阀、手爪旋转油缸伺服阀、手爪开合油缸伺服阀、回转油缸、大臂油缸、前臂油缸、手爪旋转油缸、手爪开合油缸、传感器***。

主操纵杆的模拟量数据输出作为控制指令输入给PLC主控制器，同时机械臂关节的运动位移通过高精度直线传感器反馈给PLC主控制器，两路模拟量进入主控制器后经过姿态解析算法计算出伺服阀驱动数据，PLC主控制器通过控制伺服阀的流量来控制液压执行器的动作位移，最终实现对机械臂的位置伺服控制。

所述主操纵杆有X、Y两个运动自由度，能同时进行X、Y方向的运动控制信号输出，数据信号为-10V～10V的模拟电压。操纵杆X自由度控制辅助机械臂的基座关节左右旋转；操纵杆Y自由度控制辅助机械臂大臂和前臂的两轴联动，从而确保机械臂末端的水平前后与垂直上下运动，具体运动形式由扳机开关S4控制。当扳机S4摁下时，Y自由度控制辅助机械臂末端作水平前后运动；当扳机S4松开时，Y自由度控制辅助机械臂末端作垂直上下运动。按钮S1还可以控制机械臂的单轴运动，当选择单轴运动时时，扳机开关S4摁下，则大臂作单轴旋转运动，S4不摁下，则前臂作单轴旋转运动。

PLC主控制器采用基恩士PLC KV-N24DT型，运行速度10000步/ms，可以采用梯形图+脚本(支持C语言模式)编程。

模拟量采集模块采用1个四路模拟量输入扩展模块KV-NC4AD，采集电压范围0～10V，转换速度80μs/ch。

模拟量输出模块采用3个两路模拟量输出扩展模块KV-NC2DA，，输出电压范围0～10V，转换速度80μs/ch。

主操纵杆有X、Y两个运动自由度，数据信号为0～10V的模拟电压，并具有S1、S2、S3、 S4的数字开关。

直线位移传感器选用非接触式磁致伸缩位移传感器BTL6-A500-E2/E28-KA，供电电压 10～30V，输出电压范围0～10V。

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

参见图3，为大臂和前臂运动的分析简图，由两个关节组成，大臂和前臂长度分别a₁和 a₂，两旋转关节轴平行，关节II运动范围θ₁为85°～95°，关节III运动范围θ₂为75°～135°，以基座为坐标系为{A}；关节III所在的坐标为坐标系为{B}；先求解机械臂末端在基座坐标系{A}中的位置，然后再求解θ₁、θ₂的大小。

具体的设计参数如表1所示。

表1平面两自由度串联机械臂连杆的设计参数

i	θ	关节变量角度	连杆长度	液压缸参数
					1	θ<sub>1</sub>	85°～95°	1250mm	380mm(行程100mm)
2	θ<sub>2</sub>	75°～135°	1800mm	680mm(行程400mm)

求解机械臂末端在坐标系{A}中的位置过程如下：

(1)设机械臂末端在坐标系{A}中的位置为

(2)由已知条件可求得p点在坐标系{B}中的位置为

(3)由已知条件可知其中：

(4)由(1)、(2)、(3)可得：

(5)所以：

给出机械臂末端在坐标系{A}中的位置根据机械臂杆长a₁、a₂和公式(5)即可求出相应的θ₁和θ₂，解得：复位时给定机器人末端位置：

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

总控***操作例程，***上电按钮按下，下一步***复位，读取线性传感器数据，接收控制指令，如果控制指令是单轴运动，根据机械臂单轴运动θ1和θ2数据计算机械臂末端位置(x,y),如果两轴联动，根据机械臂末端坐标(x,y)通过方程组(1)计算θ1和θ2。执行运行程序完成后，返回读取线性传感器数据循环运行。

参见图5(1)-图5(4)，该操纵杆有X、Y两个运动自由度，能同时进行X、Y方向的运动控制信号输出，数据信号为-10V～10V的模拟电压。操纵杆X自由度控制辅助机械臂的基座关节左右旋转；操纵杆Y自由度控制辅助机械臂大臂和前臂的两轴联动，从而确保机械臂末端的水平前后与垂直上下运动，具体运动形式由扳机开关S4控制。当扳机S4摁下时，Y自由度控制辅助机械臂末端作水平前后运动；当扳机S4松开时，Y自由度控制辅助机械臂末端作垂直上下运动。按钮S1还可以控制机械臂的单轴运动，当选择单轴运动时时，扳机开关 S4摁下，则大臂作单轴旋转运动，S4不摁下是，前臂作单轴旋转运动。

本申请中，PLC主控制器采用基恩士PLC KV-N24DT型，运行速度10000步/ms，可以采用梯形图+脚本(支持C语言模式)编程。

所述模拟量采集模块采用1个四路模拟量输入扩展模块KV-NC4AD，采集电压范围0～10V，转换速度80μs/ch。

所述模拟量输出模块采用3个两路模拟量输出扩展模块KV-NC2DA，，输出电压范围0～10V，转换速度80μs/ch。

主操纵杆有X、Y两个运动自由度，数据信号为0～10V的模拟电压，并具有S1、S2、S3、 S4的数字开关。

直线位移传感器选用非接触式磁致伸缩位移传感器BTL6-A500-E2/E28-KA，供电电压 10～30V，输出电压范围0～10V。

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

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

Claims (3)

1.一种辅助机械臂控制***的控制方法，所述控制***包括主操纵杆和主控制器，所述的主控制器的输入端连接模拟量采集模块和数字量采集模块，其输出端设有模拟量输出模块，所述的主操纵杆与模拟量采集模块、数字量采集模块相连，所述的模拟量输出模块输出的模拟量通过伺服放大器放大后传送给多个液压伺服阀，所述的多个液压伺服阀控制辅助机械臂运动，且辅助机械臂的回转油缸上的限位传感器采集的信号反馈给主控制器，辅助机械臂的摆动油缸上的直线位移传感器采集信号反馈给主控制器，主控制器再根据反馈信号对辅助机械臂进行精确控制；所述的辅助机械臂包括底座、大臂、前臂和手爪，在所述的底座上设有一个液压摆动油缸，液压摆动油缸驱动大臂底座在水平面内180度旋转，所述的大臂底座与大臂通过关节II相连，且大臂底座上铰接一个驱动大臂做俯仰运动的俯仰油缸I，所述的大臂的末端通过关节III铰接与其垂直的前臂，所述的前臂在俯仰油缸II的驱动下做俯仰运动；在前臂的末端通过关节IV连接一个手爪；所述俯仰油缸I带有直线位移传感器I，其通过伺服阀精确控制大臂的俯仰运动；所述俯仰油缸II带有直线位移传感器II，其通过伺服阀精确控制前臂的俯仰运动，其特征在于，如下：

主操纵杆的模拟量数据输出作为控制指令输入给主控制器；主控制器控制机械臂运动；机械臂的运动位移通过直线位移传感器反馈给主控制器，直线位移传感器I、直线位移传感器II的采集的数据进入主控制器后经过姿态解析算法计算出伺服阀驱动数据分别驱动伺服阀I、伺服阀II使关节II、关节III运动到角度θ₁、θ₂，θ₁、θ₂为关节变量角度，主控制器通过控制伺服阀的流量来控制液压执行器的动作位移，最终实现对机械臂的位置伺服控制。

2.利用权利要求1所述的控制方法，其特征在于，所述的姿态解析算法具体如下：

设大臂长度为a₁、前臂长度为a₂，关节II、关节III的关节轴平行，关节II的旋转角度为θ₁；关节III的旋转角度为θ₂；以基座为坐标系为{A}；关节III所在的坐标为坐标系为{B}；先求解机械臂末端在基座坐标系{A}中的位置，然后再求解θ₁、θ₂的大小。

3.利用权利要求2所述的控制方法，其特征在于，具体如下：

步骤(1)设机械臂末端在坐标系{A}中的位置为

步骤(2)由已知条件求得机械臂末端在坐标系{B}中的位置为

步骤(3)为关节II、关节III的大臂底座、大臂、前臂坐标系建立3×3奇次变换矩阵表示与前一个部件坐标系的关系；由已知条件可知其中：

步骤(4)由步骤(1)、(2)、(3)可得：

步骤(5)求得：

步骤(6)求得机械臂末端在坐标系{A}中的位置根据机械臂杆长a₁、a₂和机械臂末端在坐标系{A}中的位置，即可求出相应的θ₁和θ₂，解得：

上述的 ^Ap_B均表示中间参数。