CN112659120A

CN112659120A - 一种液压作业机械臂的主从遥操作及力反馈控制方法

Info

Publication number: CN112659120A
Application number: CN202011440272.5A
Authority: CN
Inventors: 荣学文; 赵昊宁; 马昕; 李贻斌; 李明; 宋锐; 田国会
Original assignee: Shandong University
Current assignee: Shandong University
Priority date: 2020-12-11
Filing date: 2020-12-11
Publication date: 2021-04-16

Abstract

本发明公开了一种液压作业机械臂的主从遥操作及力反馈控制方法，其技术方案为：包括建立液压作业机械臂DH坐标系，获取连杆参数，并得到各连杆间的旋转变换矩阵；基于旋转变换矩阵计算液压作业机械臂雅可比矩阵；根据主操作臂末端位姿信息计算从操作臂末端位姿，并根据从操作臂受力信息反馈至主操作臂，以建立液压作业机械臂主从遥操作控制模型；根据主操作臂、从操作臂之间的力信息交互，并基于雅可比矩阵建立液压作业机械臂力反馈控制模型。本发明能够根据作业机械臂受力情况输出触觉力反馈，能够更好的满足液压作业机械臂在带电作业下的控制需求。

Description

一种液压作业机械臂的主从遥操作及力反馈控制方法

技术领域

本发明涉及液压机械臂领域，尤其涉及一种液压作业机械臂的主从遥操作及力反馈控制方法。

背景技术

随着工业技术的迅速发展，工业机器人在装配、焊接、搬运等行业得到广泛应用，并且实现了自动控制，无需人员的参与。但在配网带电作业等非结构环境、多样化任务作业中，仍很难实现机器人的自动作业，仍需要人员参与控制，我国的配网带电作业方式仍是人工带电作业，操作人员作业时处于高电压、高空环境中，作业条件恶劣，容易引发人身伤亡事故。液压作业机械臂面对配电网带电作业场景，可替代人工进行部分带电作业操作，减轻工作人员劳动强度，保障人员安全。

液压作业机械臂具有6自由度机械臂，在自主控制作业的过程中完成带电作业。液压作业机械臂的作业任务具有承载量大、加速度高的一般特征，致使一个工作循环内的平均负荷很大，工作过程中的运动误差会引起各驱动单元间的非同步现象，造成受力不均甚至瞬时载荷方向相反，引起驱动单元在一个工作循环内的平均载荷激増，严重损坏传动系统寿命，因此作业过程中力度不合适可能造成机器人或配电线路损坏，所以需要对机器人进行主从遥操作和力反馈控制。

主从遥操作技术可在非确定环境下进行复杂的操纵作业临场感技术为人-机器人间和机器人-环境间创建了一种和谐的多维信息交互环境，在较高级的主从系统中广泛采用力反馈控制技术,使操作者对所操纵作业的过程具有真实的力觉感受。

发明内容

针对现有技术存在的不足，本发明的目的是提供一种液压作业机械臂的主从遥操作及力反馈控制方法，能够根据作业机械臂受力情况输出触觉力反馈，能够更好的满足液压作业机械臂在带电作业下的控制需求。

为了实现上述目的，本发明是通过如下的技术方案来实现：

本发明的实施例提供了一种液压作业机械臂的主从遥操作及力反馈控制方法，包括：

建立液压作业机械臂DH坐标系，获取连杆参数，并得到各连杆间的旋转变换矩阵；

基于旋转变换矩阵计算液压作业机械臂雅可比矩阵；

根据主操作臂末端位姿信息计算从操作臂末端位姿，并根据从操作臂受力信息反馈至主操作臂，以建立主从遥操作控制模型；

根据主操作臂、从操作臂之间的力信息交互，并基于雅可比矩阵建立力反馈控制模型。

作为进一步的实现方式，建立主从遥操作控制模型的方法为：

控制主操作臂，使其末端运动到期望的位姿P_M，主操作臂控制器获取角度信息并计算各关节的角度值

从操作臂控制器获取角度值，根据主操作臂、从操作臂末端位置比例K_P得到从操作臂末端期望的位姿

并计算从操作臂各关节的角度值

从操作臂控制器控制从操作臂各关节运动到位姿P_S。

作为进一步的实现方式，主操作臂运动到位姿P_M时，通过角位移传感器获取主操作臂各关节的角度θ_M；主操作臂控制器通过AD转换器读取角位移传感器的输出值，计算得出各关节此时的角度值

作为进一步的实现方式，从操作臂控制器基于角度值

并根据主操作臂正运动学

计算出主操作臂末端位置

得出从操作臂末端期望的位姿

后，通过从操作臂运动学逆解

计算得出从臂各关节的角度值

作为进一步的实现方式，当从操作臂受力F_S时，计算出力矩

计算出从操作臂末端受力

并根据主操作臂、从操作臂末端力比例K_F计算出期望的主操作臂末端力

作为进一步的实现方式，以各关节力矩

作为给定值，通过主操作臂控制器控制主操作臂各关节力矩，进而使主操作臂末端产生对应力F_M。

作为进一步的实现方式，通过从操作臂逆雅可比矩阵

计算出从操作臂末端受力

通过主操作臂雅可比矩阵J_M计算出与之相对应的各关节力矩

作为进一步的实现方式，建立力反馈控制模型的方法为：

主操作臂向从操作臂发送关节位置控制信号，从操作臂通过空间位置匹配进行位置控制；从操作臂获取三维力，然后将末端三维力通过转换得到主操作臂关节扭矩，并发送至主操作臂控制器。

作为进一步的实现方式，从操作臂在进行力反馈时，首先通过三维力传感器检测末端三维力，然后通过坐标转换将末端三维力由末端坐标系转换到基坐标系中；将转化后的三维力经过从操作臂和主操作臂的匹配，通过主操作臂力雅克比计算得到关节驱动力τ。

作为进一步的实现方式，计算出主操作臂各个关节扭矩τ之后，将其按照设定比例发送至主操作臂控制端。

上述本发明的实施例的有益效果如下：

(1)本发明的一个或多个实施方式的主从遥操作方法与现有传统配网机器人相比，由于应用了液压驱动技术使得机械臂绝缘性强且机械臂末端持重负载大，并且本发明的主从遥操作方法可实现人工操作的带电作业工艺，所以能够更好的满足液压作业机械臂在带电作业下的控制需求；

(2)本发明的一个或多个实施方式的力反馈控制方法与现有传统配网机器人相比，由于在从操作臂上增加力传感器和主操作臂上输出的触觉力反馈控制，使得机器人可以根据作业机械臂受力情况输出触觉力反馈，以引导操作者进行操控，大幅度减轻了操作者在复杂环境下操作机器人时的工作压力，并能有效防止作业过程中因力度不合适造成机器人或配电线路损坏。

附图说明

构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

图1是本发明根据一个或多个实施方式的控制流程图；

图2是本发明根据一个或多个实施方式的DH坐标系示意图；

图3是本发明根据一个或多个实施方式的主从遥操作控制流程图；

图4是本发明根据一个或多个实施方式的力反馈控制控制流程图。

具体实施方式

应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

实施例一：

本实施例提供了一种液压作业机械臂的主从遥操作及力反馈控制方法，包括：

基于旋转变换矩阵计算液压作业机械臂雅可比矩阵；

进一步的，如图1所示，主操作臂控制器的输出端与主操作臂之间依次通过DA转换器连接运算放大器、OCL电机驱动、直流电机；主操作臂控制器的输入端与主操作臂之间依次连接AD转换器、角位移传感器。

主操作臂控制器用于获取主操作臂各关节当前角度值，并实现主操作臂末端力反馈。角度值获取通过AD转换器读取角位移传感器输出端的电压值，再通过计算得出。主操作臂末端力反馈是通过期望的末端力计算得出各关节需要输出的力矩值，进而将主操作臂末端的力控制转化为主操作臂各关节的力矩控制。

具体的，包括以下步骤：

步骤1：建立液压作业机械臂DH坐标系、DH参数和连杆间的旋转变换矩阵。

进一步的，采用Denavit Hartenberg(DH)方法建立坐标系，主、从操作臂的坐标系相同，如图2所示；建立DH坐标系之后，根据主操作臂、从操作臂的设计要求，得出连杆参数a_i-1、α_i-1、d_i、θ_i，其中a_i-1表示关节轴i-1和关节轴i之间的连杆长度，α_i-1表示关节轴i-1和关节轴i之间的扭转角，d_i表示关节轴i的连杆偏距，θ_i表示关节轴i的关节角。

根据液压作业机械臂的连杆参数可得出各连杆间的旋转变换矩阵如下：

式中，sθ_i＝sinθ_i，cθ_i＝cosθ_i，s_i＝sinθ_i，c_i＝cosθ_i，s₂₃＝sin(θ₂+θ₃)，c₂₃＝cos(θ₂+θ₃)，s₂₃₄＝sin(θ₂+θ₃+θ₄)，c₂₃₄＝cos(θ₂+θ₃+θ₄)。

步骤2：计算液压作业机械臂雅可比矩阵J。

进一步的，各连杆坐标系{O_i}的z轴单位向量在基坐标系{O₀}中的表示为：

操作臂末端关节坐标系原点相对于坐标系{O_i}的位置矢量：

操作臂末端关节坐标系原点相对坐标系{O_i}的位置矢量在基坐标系{O₀}中的表示：

由转动关节的雅可比矩阵求解方法可以计算各列雅可比列向量如下：

液压作业机械臂雅克比矩阵为：

J＝[J₁ J₂ J₃ J₄ J₅ J₆] (2-25)

步骤3：建立液压作业机械臂主从遥操作控制模型。

进一步的，如图3所示，控制主操作臂使其末端运动到期望的位姿P_M，则主操作臂各关节的角度θ_M会随之发生变化，从而引起各关节角位移传感器的输出值发生变化。主操作臂控制器通过AD转换器读取各传感器的输出值，计算得出各关节此时的角度值

并通过串行通信接口将计算得出的各关节角度参数

发送给从操作臂控制器。

从操作臂控制器根据主操作臂正运动学

计算出主操作臂末端位置

然后根据主、从操作臂末端位置比例K_P，计算得出从操作臂末端期望的位姿

之后通过从操作臂运动学逆解

计算得出从臂各关节的角度值

从操作臂控制器通过控制的从操作臂角度，使其运动到位姿P_S。

当从操作臂末端有负载或者从操作臂与作业环境接触时，从操作臂受力为F_S，使得从操作臂各关节也受到相应的力矩τ_S。通过安装在从操作臂各关节的力传感器，可以测出相应的力矩

由于从操作臂运动较慢，当忽略从操作臂运动时，通过从操作臂逆雅可比矩阵

计算出从操作臂末端受力

进而根据设计的主、从操作臂末端力比例K_F，计算出期望的主操作臂末端力

然后通过主操作臂雅可比矩阵J_M计算出与之相对应的各关节力矩

以其作为给定值，通过主操作臂控制器控制主操作臂各关节力矩，进而使主操作臂末端产生对应力F_M。从而实现主操作臂控制从臂的运动轨迹，并且成比例的反馈从操作臂末端受力。

步骤4：建立液压作业机械臂力反馈控制方法模型。

力反馈的实现可以看成是双向信息交互的结果。进一步的，如图4所示，首先主操作臂在控制系统的作用下向从操作臂发送关节位置控制信号，从操作臂通过空间位置匹配进行位置控制。从操作臂通过末端三维力传感器检测三维力，然后将末端三维力通过转换得到主操作臂关节扭矩，发送给主操作臂控制器。

实现力反馈控制，主、从操作臂的信息交互有两条通道：一是主操作臂到从操作臂的位置控制信息；二是从操作臂到主操作臂的力反馈信息。从操作臂在进行力反馈时，首先通过三维力传感器检测末端三维力，然后通过坐标转换将末端三维力由末端坐标系转换到基坐标系中，将转化后的三维力经过从操作臂和主操作臂的匹配，最后通过主操作臂力雅克比计算得到关节驱动力为：

τ＝J^TF₆ (4-1)

式中，J^T为主操作臂雅克比矩阵的转置，τ＝[τ₁ τ₂ τ₃ τ₄ τ₅ τ₆]^T为六自由度从操作臂各个关节的扭矩，F₆为从臂末端在接触地方产生力f₆和力矩n₆的比例转换。

在主操作臂的操作过程中，对操作臂摆动速度必须在很慢的情况下进行读取，对于力反馈的实验结果，只考虑操作臂静止的情况。对于控制模块的快速性，采用串行通信将采用每秒不低于100次的运算速度进行信息交互；对于准确性，要求控制程序能够准确的对力信息进行反馈；对于控制模块的稳定性，要求在力反馈的过程中，避免反馈力的震动，为实现这一要求，采用卡尔曼滤波的方法对从臂末端采集的三维力信息进行滤波。

计算出主操作臂各个关节扭矩τ之后，将该数据按照比例以串口通信的形式发送到主操作臂控制端，继而通过主操作臂来感受来自从操作臂末端的力。

以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。