CN104669254B - 一种双串联机械臂式移动操作机装置的自动化定位方法 - Google Patents

Abstract

一种双串联机械臂式移动操作机装置的自动化定位方法，双串联机械臂式移动操作机装置由车身和两个串联式机械臂组成，两个串联式机械臂安装在车身的前部上面左右对称布置，定位的执行部件为两个串联式机械臂，串联式机械臂主要包括：末端操作器（1），滑块（2），推进油缸（3），滑架（4），整臂摆动油缸（5），升降油缸（6），大臂（7），俯仰油缸（8），摆角油缸（9）。本发明的方法利用机器人学原理求取装置的运动学逆解解析解表达式，根据定位作业方案获取装置定位控制所需的串联式机械臂的关节坐标，从而实现装置的自动定位。多个定位作业任务可以采用双机械臂同时动作或者分时动作。

Description

一种双串联机械臂式移动操作机装置的自动化定位方法

技术领域

本发明涉及一种自动化定位方法，尤其是一种双串联机械臂式移动操作机装置的自动化定位方法。

背景技术

目前，国内外市场上矿用掘进钻车多为双串联机械臂式液压钻车，操作方式多为人工操作，普遍存在钻杆定位困难、操作不便，效率低下等问题。常见的定位操作就是钻孔，而计算机控制的自动化钻孔能有效解决井下人工凿岩采矿带来的安全、效率和钻爆精度等问题。具有智能化与自主操作的井下自动化凿岩设备是当今矿业研究的热点和难点。其中，保证钻车能够在具有多个拐角的巷道中间快速行进且不碰触到墙面，并能进行精确实时的定位和导航，实现自动化钻孔，是矿业研究的一个重要方向。因此，对双串联机械臂式移动操作机装置的自动化定位方法进行研究对于矿用钻车实现自动化钻孔等精确定位作业，提高作业效率和作业水平具有重要意义。

发明内容

基于以上目的，本发明提出一种自动定位方法，定位的执行部件为安装在一个移动操作机上的两个串联式机械臂，具体方法如下：

首先用机器人运动学的理论得到双串联机械臂式移动操作机的运动学逆解解析解表达式，根据定位作业方案的用户坐标得到串联式机械臂的关节坐标，其中为第1个运动自由度，实现机械臂的摆动；为第2个自由度，实现机械臂的俯仰；为第3个自由度，实现机械臂的回转；为第4个自由度，实现滑架俯仰；为第5个自由度，实现滑架摆动；为第6个自由度，实现滑块的移动。

由串联式机械臂实现定位操作，首先整臂摆动油缸5伸缩，实现机械臂绕竖直回转轴Z0的回转，到达关节位置度；升降油缸6伸缩实现机械臂的俯仰，到达关节位置度；液压马达经蜗轮蜗杆减速后驱动大臂7的回转部分回转，实现机械臂的回转，到达关节位置度；俯仰油缸8伸缩实现滑架俯仰，到达关节位置度；摆角油缸9伸缩实现滑架摆动，到达关节位置度；最后推进油缸3伸缩，实现滑块2的直线运动，到达位置。

完成多个定位作业任务可以两个串联式机械臂同时动作也可以分时动作，实现末端操作器1的定位操作。

本发明的双串联机械臂式移动操作机装置的自动化定位方法由机器人运动学的理论得到双串联机械臂式移动操作机的运动学解析解表达式，根据定位作业方案的用户坐标得到串联式机械臂的关节坐标，实现根据用户定位作业方案的数据信息获取移动操作机两个串联式机械臂的关节坐标信息，通过控制两个串联式机械臂6个自由度的动作使两个串联式机械臂的末端操作器达到用户期望的位姿，从而实现移动操作机作业装置的自动定位。

附图说明

图1是本发明的双串联机械臂式移动操作机装置的串联式机械臂结构示意图；

图2是本发明的双串联机械臂式移动操作机装置的运动学坐标系和用户坐标系。

具体实施例

本发明的双串联机械臂式移动操作机装置由车身和两个串联式机械臂组成，两个串联式机械臂安装在车身的前部上面左右对称布置。

图1所示为串联式机械臂，主要包括：末端操作器1，滑块2，推进油缸3，滑架4，整臂摆动油缸5，升降油缸6，大臂7，俯仰油缸8，摆角油缸9。

大臂7的固定部分和双串联机械臂式移动操作机的机身铰接，大臂7的回转部分和滑架4铰接。升降油缸6布置在大臂7的下面后侧，升降油缸6的后侧和整臂摆动油缸5铰接、升降油缸6的前侧和大臂7的固定部分铰接。俯仰油缸8布置在大臂7的下面前侧，俯仰油缸8的后侧和大臂7的回转部分铰接，俯仰油缸8的前侧和滑架4铰接。摆角油缸9布置在大臂7的前侧，摆角油缸9的后侧和大臂7的回转部分铰接，摆角油缸9的前侧和滑架4铰接。滑架4布置在大臂7的上部，滑架4的下面后侧和大臂7的回转部分铰接，滑架4的下面前侧和摆角油缸9的前侧铰接。推进油缸3布置在滑架4的上部后侧。滑块2布置在滑架4的上部，滑块2的后部和推进油缸3的前部铰接。滑块2和滑架4形成移动副。

左侧和右侧的串联式机械臂结构基本相同，两个机械臂的区别在于：左边的串联式机械臂的整臂摆动油缸5布置在机械臂左侧，右边的串联式机械臂的整臂摆动油缸5布置在机械臂右侧。

双串联机械臂式移动操作机装置运动学计算的目的是求解末端操作器的期望位姿对应的关节变量，末端操作器的位姿为6个自由度，需要6轴联动。这里建立双串联机械臂式移动操作机装置的运动学坐标系和用户坐标系如图2所示。

两个串联式机械臂D-H参数中的常量部分参数完全一致。根据建立的坐标系确定串联式机械臂的D-H参数如表1所示：

表1串联式机械臂各连杆D-H参数

坐标变换的A矩阵表达式为

以和分别代表和，简写后

，，

，，

，

式中：c₁=cos；s₁=sin；c_b1=cos；s_b1=sin。c₂，s₂，c₃，s₃，c₄，s₄，c₅，s₅，c_b2，s_b2，c_b3，s_b3同理。

A_b为车姿矩阵，车姿矩阵中的参数可由激光导向以特殊方法计算得到，部分参数也可以由特殊安装的传感器得到。

建立双机械臂统一的运动学方程如下：

求解此结构型式的运动学逆解，采用分离变量法求得解析解，求解过程如下：

用、、左乘(1)式，有。

令，，有

将矩阵左右两边展开

通过观察矩阵内元素之间的关系，可以得到

通过(2)式两边矩阵对应元素相等，有如下关系式

(2)式两边矩阵第一行的第一列与第三列相等有

令，得，解得

(2)式两边矩阵第一行第二列相等有

(3)式与联立得

令，得，解得

即

将k₂、m带入式中得

同理可得

由以上结果可求得、、

即

联立(4)、(5)、(6)成方程组得

解方程组(11)可求得，，，那么所有变量的解析解表达式便可全部求出。

下面求解，，，通过解(11)求得，，的解析解的表达式。

对于这个方程组的求解，需要分五种情况讨论，；；；；以及除了前四种情况的其他情况，对于前四种情况，命名为特殊情况，除了特殊情况的其他情况则为一般情况。下面分析每一种情况，首先分析一般情况。

一、一般情况求解过程如下：

(1-1)*a₂₃-(1-2)*a₁₃与(1-2)*a₃₃-(1-3)*a₂₃联立，消去d₆得

令

得

通过(1-4)，有，将其带入(1-5)，得

将(1-6)展开，得

令

得

利用万能公式、，将(1-7)展开，再令，整理得

令

有

这是一个一元四次方程，利用“一般实系数四次方程的谢国芳求根公式”可求得的最终解，结果如下：

首先定义参数

以及

当、、全为正数时，如果，，否则，；当、、不全为正数时，如果，，否则，；则。

由(1-4)+(1-5)，得，解得

，则；

由(1-1)有；

至此、、全部求出。

二、当a₂₁=a₂₃=0时

由(2-1)可得

由(2-2)可得

即

(2-1)与(2-3)联立成方程组为

解得

即

三、当a₂₂=a₂₃=0时

(3-1)*a₃₃-(3-3)*a₁₃得

(3-4)与(3-2)联立方程组为

令

得

之后解法同一般情况。

四、当a₃₂=a₃₃=0时

(4-1)*a₂₃-(4-2)*a₁₃得

(4-4)与(4-3)联立方程组为

令

得

之后解法同一般情况。

五、当a₂₃=a₃₃=0时

(5-2)与(5-3)联立成方程组为

令

得

之后解法同一般情况。

至此，双串联机械臂式移动操作机装置运动学逆解的解析解表达式全部求出。通过MATLAB工具进行正解反解一致性的判断，结果表明正解反解一致。并利用c语言编程软件VC++6.0将逆解编写成了程序。

首先根据特定的用户定位方案来获取定位数据，也就是得到的用户坐标。这里假定末端操作器在用户坐标系中的位姿为T，其中第1列为末端操作器x轴姿态、第2列为末端操作器y轴姿态、第3列为末端操作器z轴姿态，第4列为末端操作器的位置。

假定末端操作器的目标位姿为：

车姿矩阵为：

把数据输入程序，输入形式：，，，，，，，，，，，以及，，，，，，，，，，，。

得出结果，，，，，。其中为第1个运动自由度，实现机械臂的摆动；为第2个自由度，实现机械臂的俯仰；为第3个自由度，实现机械臂的回转；为第4个自由度，实现滑架俯仰；为第5个自由度，实现滑架摆动；为第6个自由度，实现滑架的移动。

通过编程好的程序计算出上述关节角度后，就可以通过控制系统控制关节运动使机械臂的末端操作器到达目标位姿。

多个定位作业可以双机械臂同时动作也可以分时动作，关键需要做好动作规划。单个串联式机械臂的运动可以由两种方式实现：一种是单自由度顺序运动，共6次完成定位工作；另一种是多个自由度联动共同完成定位工作。

下面以单个串联式机械臂单自由度运动方式为例介绍定位实现方法：

移动操作机的定位作业由左侧串联式机械臂实现定位，首先整臂摆动油缸5伸缩，实现机械臂绕竖直回转轴Z0的回转，到达关节位置度；升降油缸6伸缩实现机械臂的俯仰，到达关节位置度；液压马达经蜗轮蜗杆减速后驱动大臂7的回转部分回转，实现机械臂的回转，到达关节位置度；俯仰油缸8伸缩实现滑架俯仰，到达关节位置度；摆角油缸9伸缩实现滑架摆动，到达关节位置度；最后推进油缸3推动末端操作器1直线运动，到达位置mm，到此就完成定位工作。

Claims (2)

1.一种双串联机械臂式移动操作机装置的自动化定位方法，双串联机械臂式移动操作机装置由车身和两个串联式机械臂组成，两个串联式机械臂安装在车身的前部上面左右对称布置，定位的执行部件为两个串联式机械臂，串联式机械臂主要包括：末端操作器（1），滑块（2），推进油缸（3），滑架（4），整臂摆动油缸（5），升降油缸（6），大臂（7），俯仰油缸（8），摆角油缸（9）；大臂（7）的固定部分和双串联机械臂式移动操作机的机身铰接，大臂（7）的回转部分和滑架（4）铰接，升降油缸（6）布置在大臂（7）的下面后侧，升降油缸（6）的后侧和整臂摆动油缸（5）铰接、升降油缸（6）的前侧和大臂（7）的固定部分铰接，俯仰油缸（8）布置在大臂（7）的下面前侧，俯仰油缸（8）的后侧和大臂（7）的回转部分铰接，俯仰油缸（8）的前侧和滑架（4）铰接，摆角油缸（9）布置在大臂（7）的前侧，摆角油缸（9）的后侧和大臂（7）的回转部分铰接，摆角油缸（9）的前侧和滑架（4）铰接，滑架（4）布置在大臂（7）的上部，滑架（4）的下面后侧和大臂（7）的回转部分铰接，滑架（4）的下面前侧和摆角油缸（9）的前侧铰接，推进油缸（3）布置在滑架（4）的上部后侧，滑块（2）布置在滑架（4）的上部，滑块（2）的后部和推进油缸（3）的前部铰接，滑块（2）和滑架（4）形成移动副，左侧和右侧的串联式机械臂结构基本相同，两个机械臂的区别在于：左边的串联式机械臂的整臂摆动油缸（5）布置在机械臂左侧，右边的串联式机械臂的整臂摆动油缸（5）布置在机械臂右侧，其特征在于采用如下方法进行自动化定位：首先用机器人运动学的理论得到双串联机械臂式移动操作机的运动学逆解解析解表达式，根据定位作业方案的用户坐标得到串联式机械臂的关节坐标，其中θ₁为第1个运动自由度，实现机械臂的摆动；θ₂为第2个自由度，实现机械臂的俯仰；θ₃为第3个自由度，实现机械臂的回转；θ₄为第4个自由度，实现滑架俯仰；θ₅为第5个自由度，实现滑架摆动；d₆为第6个自由度，实现滑块的移动；首先整臂摆动油缸（5）伸缩，实现机械臂绕竖直回转轴Z0的回转，到达关节位置度θ₁；升降油缸（6）伸缩实现机械臂的俯仰，到达关节位置度θ₂；液压马达经蜗轮蜗杆减速后驱动大臂（7）的回转部分回转，实现机械臂的回转，到达关节位置度θ₃；俯仰油缸（8）伸缩实现滑架俯仰，到达关节位置度θ₄；摆角油缸（9）伸缩实现滑架摆动，到达关节位置度θ₅；最后推进油缸（3）伸缩，实现滑块（2）的直线运动，到达位置d₆。

2.根据权利要求1所述的一种双串联机械臂式移动操作机装置的自动化定位方法，其特征在于：多个定位作业任务可以采用双机械臂同时动作或者分时动作。