CN103056883B - 一种双机械臂协调控制***及控制方法 - Google Patents

Abstract

本发明公开了一种双机械臂协调控制***，包括控制键盘、主操纵手、显示器、主控制柜、两个从控制柜，本发明通过采集键盘与主操纵手的脉冲数据，以指令形式发送给下位机，从而完成对两个机械臂的运动控制。主控制柜的工控机是连接主操纵手的计算机，通过串口和控制机械臂的从控制柜的工控机进行通讯，发出运动指令。从控制柜的工控机根据主控制柜的工控机的运动指令对机械臂的运动进行规划。本发明利用联接在其末端的夹持手、专用工具，代替人的手臂完成10KV线路带电作业频率较高的带电断线、带电接线、带电更换跌落保险等任务，一方面可减轻作业人员的劳动强度，另一方面也为作业人员的安全提供了保证。

Description

一种双机械臂协调控制***及控制方法

技术领域

本发明涉及一种机器人技术，尤其是一种双机械臂协调控制***及控制方法。

背景技术

在带电作业技术发展阶段中带电作业方式都可以说是人工带电作业，即操作人员作业时都时刻处于高电压、强电场的威胁中。人工带电作业一般情况下是高空作业，登杆作业频繁、作业条件恶劣、劳动强度大、精神紧张，容易引发人身伤亡事故。据1995年全国100次典型带电作业事故统计中，配电带电作业事故达55次，达55％，在死亡的39人中有19人死于配电带电作业事故，占61％。所以，在过去很长的一段时间内，对配电带电作业进行了限制，致使配电线路停电作业频繁，配电可靠性指标不能完成，从而给电力企业带来了很大的经济损失，给人民生活和生产带来了很大的不便。因此，在1987年召开的全国带电作业会议上，又提出了必须大力加强配电带电作业的要求。

国内在机器人方面的研究，自80年代中后期，由于国家高技术研究发展计划（即863计划）专项支持，得到了极大发展，已成为世界上研究机器人的主要国家之一。从我国已经形成的研究体系看，基本形成了工业机器人和特种机器人两大主要门类，尤其是特种机器人的研究已成为国内的研究热点。特种机器人主要是指工作极限环境和危险环境及危害人身健康环境的机器人，由于这些环境对机器人要求的必要性和迫切性强，因而市场前景好。配电线路带电作业机器人就是很有代表性的特种机器人，因此研制具有更强的安全性和适应性的高压带电作业机器人，克服人工带电作业的困难和局限性，代替人进行带电作业非常必要，而且符合时代的要求。此种机器人可以减轻作业人员的劳动强度，使作业人员与高压电场完全隔离，最大限度的保证作业人员的安全。

发明内容

本发明的目的是为克服上述现有技术的不足，提供一种利用联接在其末端的夹持手、专用工具，代替人的手臂完成10KV线路带电作业频率较高的带电断线、带电接线、带电更换跌落保险等任务，一方面可减轻作业人员的劳动强度，另一方面也为作业人员的安全提供了保证的双机械臂协调控制***及控制方法。

为实现上述目的，本发明采用下述技术方案。

一种双机械臂协调控制***，包括控制键盘、主操纵手、显示器、主控制柜、两个从控制柜。

控制键盘的输出接主控制柜的输入，主控制柜的输入输出接显示器的输入输出，主操纵手的输出接主控制柜的输入。

主控制柜包括工控机，主控制柜的网口分别与从控制柜的网口通信。所述主控制柜的工控机采集控制键盘的指令，通过网口通信下发给从控制柜的工控机，从控制柜的工控机下发速度指令、伺服驱动指令给运动控制器。

从控制柜包括工控机、运动控制器、采集器、稳压电源、伺服电机驱动器，稳压电源分别给工控机、运动控制器、伺服电机驱动器、采集器供电；机械臂的私服电机分别与伺服电机驱动器、采集器相连；伺服电机驱动器与运动控制器相连，采集器、运动控制器与工控机相连，所述采集器读取绝对位置信息，再通过串口把这些信息传送给从控制柜的工控机；所述运动控制器接收运动控制指令，完成伺服控制功能。

所述机械臂包括结构相同的左臂和右臂，机械臂的机械结构包括基座，基座上安装有支撑臂，支撑臂上端为肩关节，上臂安装在肩关节上，上臂的前端与肘关节连接，肘关节与前臂连接，前臂的末端为腕部；支撑臂的底部安装伺服电机、伺服电机减速器，肩关节和肘关节上设有绝对值编码器。上臂包括依次设置的腰部回转轴、上臂俯仰轴和前臂俯仰轴，提供腰部回转、上臂俯仰和前臂俯仰的运动。前臂包括依次设置的腕部俯仰轴、腕部摇摆轴、腕部旋转轴，提供腕部俯仰、腕部摇摆和腕部旋转的运动。

所述主操纵手与机械臂的机械结构完全相同，主操纵手还包括正交旋转编码器和控制按钮，它主要用于主从式控制方式时，机器人位控信号输入。正交旋转编码器联接在主操纵手的各个关节中，以检测主操纵手各关节位置变化，主操纵手控制按钮用于控制主操纵手位置信号的有效性。

所述工控机采用IPC-6806S紧凑型机箱，主板采用PCA-6774主板，1GHz CPU。

所述运动控制器采用PMAC，数字PID控制算法，PID参数在***运行时可实时修改，内部梯形速度轨迹运算器，可实现自动加减速控制。

所述伺服电机驱动器的输入采用0-10V模式量。

一种双机械臂协调控制方法，包括如下步骤：

(1)输入机械臂末端在空间两点(初始点和终点)的位置和姿势，用位姿矩阵T₁，T₂表示；

$T_1=\left[\begin{array}{cccc}{n}_{x1}& o_{x1}& a_{x1}& p_{x1}\\ n_{y1}& o_{y1}& a_{y1}& p_{y1}\\ n_{z1}& o_{z1}& a_{z1}& p_{z1}\\ 0& 0& 0& 1\end{array}\right]$ -----初始位姿矩阵；

p_x1

p_y1------表示机械臂末端在世界坐标系中的初始位置；

p_z1

n_x1  o_x1  a_x1

n_y1  o_y1  a_y1------表示机械臂末端在世界坐标系中的初始姿态；

n_z1  o_z1  a_z1

$T_2=\left[\begin{array}{cccc}{n}_{x2}& o_{x2}& a_{x2}& p_{x2}\\ n_{y2}& o_{y2}& a_{y2}& p_{y2}\\ n_{z2}& o_{z2}& a_{z2}& p_{z2}\\ 0& 0& 0& 1\end{array}\right]$ -----终点位姿矩阵；

p_x2

p_y2------表示机械臂末端在世界坐标系中的终点位置；

p_z2

n_x2  o_x2  a_x2

n_y2  o_y2  a_y2------表示机械臂末端在世界坐标系中的终点姿态；

n_z2  o_z2  a_z2

(2)进行运动学反解计算，求得对应于T₁和T₂的两组关节变量值(q_o1,q_o2,.....,q_oN)，(q_f1,q_f2,......,q_fN)；

（q_o1,q_o2,.....,q_oN)------初始关节变量值；

(q_f1,q_f2,......,q_fN)-----终点关节变量值；

N--------是1到6之间的所有正整数；

(3)输入1到6轴在初始点和终点的速度、加速度、初始关节变量值(q_o1,q_o2,.....,q_oN)、终点关节变量值(q_f1,q_f2,......,q_fN)；输入抬起点和下降点的位置(q₁,q₂)以及运动时间T和各轨迹段时间Δτ_i；

(4)利用3－5－3轨迹规划公式计算机械臂1到6轴各轨迹段多项式系数；

(5)计算1到6轴在每一瞬时τ(0＜τ＜T)的位置、速度、加速度；

(6)输出机械臂1到6轴运动规律。

本发明的双机械臂控制器的工作原理是：通过采集键盘与主操纵手的脉冲数据，以指令形式发送给下位机，从而完成对两个机械臂的运动控制。主控制柜的工控机是连接主操纵手的计算机，通过串口和控制机械臂的从控制柜的工控机进行通讯，发出运动指令。从控制柜的工控机根据主控制柜的工控机的运动指令对机械臂的运动进行规划。底层的运动控制任务由基于PC总线的运动控制器来实现。私服电机采用集成绝对码盘的伺服电机，接触编码器信号接口板负责读取各关节的绝对位置信息并且同从控制柜的工控机进行通讯。

采用上述方案，本发明具有以下优点，一是自行设计的双机械臂协调***经实验验证，代替人的手臂完成10KV线路带电作业频率较高的带电断线、带电接线、带电更换跌落保险等任务；二是嵌入式机械臂自重轻、尺寸小、控制***功耗低和尺寸小，适合移动操作带电作业机器人的应用需要。三是双机械臂可以实现复杂的直线插补、圆弧插补运动；四是***采用模块化设计，具有开放性、可读性、可扩展性、可维护性，以便持续开发。五是机械臂运动控制器采用主从式工控机，主控制柜的工控机实现运动学正反解、插补算法，从控制柜的工控机实现运动控制，处理速度快。六是控制器带有驱动器控制接口、绝对值编码器采集口，功能齐全，位置精度高。

附图说明

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

图2是本发明主操纵手结构图；

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

图4是本发明机械臂坐标系示意图；

图5是本发明总程序流程图；

图6是本发明主从运动函数流程图；

图7是本发明自主运动函数流程图；

图8是本发明PTP运动轨迹规划流程图；

其中，1、控制键盘；2、主操纵手；3、显示器；4、主控制柜；5、第一从控制柜；6、第二从控制柜；7、第三工控机；8、第一工控机；9、第二工控机；10、运动控制器；11、采集器；12、稳压电源；13、伺服电机驱动器；14、左臂；15、右臂；16、基座；17、支撑臂；18、肩关节；19、上臂；20、肘关节；21、前臂；22、腕部；23、私服电机；24、伺服电机减速器；25、绝对值编码器；26、正交旋转编码器；27、控制按钮；28、手爪锁定按钮；29、停止和复位按钮；30、手爪开合按钮；31、开关状态指示灯；32、停止模式状态指示灯；33、锁定状态指示灯；34、腰部回转轴；35、上臂俯仰轴；36、前臂俯仰轴；37、腕部俯仰轴；38、腕部摇摆轴；39、腕部旋转轴。

具体实施方式

下面结合附图和实施例对本发明进一步说明。

参见图1，一种双机械臂协调控制***，包括控制键盘1、主操纵手2、显示器3、主控制柜4、第一从控制柜5；第二从控制柜6。

控制键盘1的输出接主控制柜4的输入，主控制柜4的输入输出接显示器3的输入输出，主操纵手2的输出接主控制柜4的输入。

主控制柜4包括第三工控机7，主控制柜4的网口分别与第一从控制柜5、第二从控制柜6的网口通信。所述主控制柜4的第三工控机7采集控制键盘1的指令，通过网口通信下发给第一从控制柜5的第一工控机8、第二从控制柜6的第二工控机9，第一工控机8、第二工控机9下发速度指令、伺服驱动指令给运动控制器10。

所述第一从控制柜5包括第一工控机8、运动控制器10、采集器11、稳压电源12、伺服电机驱动器13，稳压电源12分别给第一工控机8、运动控制器9、伺服电机驱动器13、采集器11供电；伺服电机驱动器13与运动控制器10相连，采集器11、运动控制器10与第一工控机8相连，所述采集器11读取绝对位置信息，再通过串口把这些信息传送给第一工控机8；所述运动控制器10接收运动控制指令，完成伺服控制功能。运动控制器10为6轴运动控制器。所述第二从控制柜6与第一从控制柜5相同，不再赘述。

参见图2，主操纵手2为6自由度，所述主操纵手2与机械臂的机械结构完全相同，主操纵手2还包括正交旋转编码器26和控制按钮27，它主要用于主从式控制方式时，机器人位控信号输入。正交旋转编码器26联接在主操纵手2的各个关节中，以检测主操纵手2各关节位置变化，主操纵手2的控制按钮27用于控制主操纵手2位置信号的有效性。主操纵手2的前臂部分上带有手爪锁定按钮28、停止和复位按钮29、手爪开合按钮30；在主操纵手2的上臂部分下端的底座面板上设有开关状态指示灯31、停止模式状态指示灯32、锁定状态指示灯33。

控制键盘1的输入功能有：机械臂的6个轴正反向可进行三个速度切换的单动；三个速度正反向的世界坐标运动和工作坐标运动；左臂14和右臂15的切换；左臂14和右臂15的工作原点复位与运输原点复位；控制键盘1/主操纵手2的切换；控制键盘1的锁死（以防止用户误动）；第一从控制柜5和第二从控制柜6的伺服关闭与打开；第三工控机7与两个下位机的关机操作；剥皮功能的程序自动控制。

所述第一从控制柜5和第二从控制柜6上均设有控制面板，所述控制面板上设有工作方式选择开关，工作方式选择开关(Mode Selection)：为三相选择开关，用于选择机械臂工作方式的切换。高压带电机械臂有三种工作方式：复位(Home)、编程(Prog)、运行(Run)。由于采用增量式编码器，每次开机时必须首先进行复位工作，否则其他一切操作均无效。当机械臂复位完毕，可进行工作方式转换，如果要对机械臂进行离线编程，机械臂必须处于编程工作方式。如果要使机械臂按照指定的运行方式运动，必须使机械臂工作在运行方式；运行方式选择开关(RunSelection)：为三相选择开关，用于选择机械臂运动方式的切换。高压带电机械臂有三种运动方式：主从运动(Ms)、单步运行(Single)和自动运行(Auto)方式。主从运动方式时，机械臂位姿可根据主操纵手位姿的变化而变化，从而达到操作者控制要求。单步运行时，机械臂按照实现编制的机械臂运动程序，每次运行一步。自动运行时，机械体按照实现编制的机械臂运动程序一次或循环自运行；急停按钮(Emergence)：当出现紧急情况时，按下急停按钮，则使伺服***挂起，退出机械臂控制程序，机械臂停止运动；运动起停按钮：为触发式按钮，用于触发机械臂运动控制的启动(Start)和停止(Stop)。

参见图3，所述机械臂为6自由度，所述机械臂包括结构相同的左臂14和右臂15，左臂14和右臂15分别连接第一从控制柜5和第二从控制柜6，左臂14和右臂15均为6自由度，左臂14的机械结构包括基座16，基座16上安装有支撑臂17，支撑臂17上端为肩关节18，上臂19安装在肩关节18上，上臂19的前端与肘关节20连接，肘关节20与前臂21连接，前臂21的末端为腕部22；支撑臂17的底部安装伺服电机23、伺服电机减速器24，私服电机23分别与伺服电机驱动器13、采集器11相连，肩关节18和肘关节20上设有绝对值编码器25。所述右臂15与左臂14相同，不再赘述。

参见图4，上臂19包括依次设置的腰部回转轴、上臂俯仰轴和前臂俯仰轴，提供腰部回转、上臂俯仰和前臂俯仰的运动。

前臂21包括依次设置的腕部俯仰轴、腕部摇摆轴、腕部旋转轴，提供腕部俯仰、腕部摇摆和腕部旋转的运动。

对每个杆件在关节轴处可建立一个正规的笛卡儿坐标系(x_i,y_i,z_i)(i是1到6之间的所有正整数，6为自由度数目)，再加上基座坐标系(x₀,y₀,z₀)（在机座上的位置和方向可任选，只要z₀轴沿着第一关节运动轴即可）。该发明确定和建立每个坐标系应根据下面三条规则：每个轴i(i是1到6之间的所有正整数，6为自由度数目)的运动都绕着z_i轴运动；x_i轴垂直z_i-1轴并指向离开z_i-1轴的方向；y_i轴按右手坐标系得要求建立。

参见图5，总控程序是机械臂软件***顺序控制的核心，是整个软件***的起点。它工作在第三工控机7上，用于控制整个机械臂软件***工作的，它主要完成***初始化、输入输出读入、***状态监测等任务。总控程序在工业控制计算机上电后便自动开始运行，只有当电源断开时，才停止运行。它采用循环检测的机制对各输入口进行定时检测，根据输入控制信号来顺序控制和执行相应模块功能。

参见图6，机械臂主要工作于主从运动方式，机械臂根据操作者所控制的主操纵手2位置和姿态的变化而变化。在设计主从运动函数时采用定时采样主操纵手2位姿变化的方法得到运动控制数据，控制机械臂的运动，以达到对主操纵手2的位姿跟随功能。因此，主从运动函数控制精度由采样时间所决定，采样时间越短控制精度越高。

参见图7，机械臂处于自动运动方式时，按照事先编写好的运动控制程序自动循环执行，直到按下控制面板上的Stop按键。

参见图8，机械臂PTP运动轨迹规划的算法如下：

(1)输入机械臂末端在空间两点(初始点和终点)的位置和姿势，用位姿矩阵T₁，T₂表示；

$T_1=\left[\begin{array}{cccc}{n}_{x1}& o_{x1}& a_{x1}& p_{x1}\\ n_{y1}& o_{y1}& a_{y1}& p_{y1}\\ n_{z1}& o_{z1}& a_{z1}& p_{z1}\\ 0& 0& 0& 1\end{array}\right]$ -----初始位姿矩阵；

p_x1

p_y1------表示机械臂末端在世界坐标系中的初始位置；

p_z1

n_x1  o_x1  a_x1

n_y1  o_y1  a_y1------表示机械臂末端在世界坐标系中的初始姿态；

n_z1  o_z1  a_z1

$T_2=\left[\begin{array}{cccc}{n}_{x2}& o_{x2}& a_{x2}& p_{x2}\\ n_{y2}& o_{y2}& a_{y2}& p_{y2}\\ n_{z2}& o_{z2}& a_{z2}& p_{z2}\\ 0& 0& 0& 1\end{array}\right]$ -----终点位姿矩阵；

p_x2

p_y2------表示机械臂末端在世界坐标系中的终点位置；

p_z2

n_x2  o_x2  a_x2

n_y2  o_y2  a_y2------表示机械臂末端在世界坐标系中的终点姿态；

n_z2  o_z2  a_z2

(2)进行运动学反解计算，求得对应于T₁和T₂的两组关节变量值(q_o1,q_o2,.....,q_oN)，(q_f1,q_f2,......,q_fN)；

(q_o1,q_o2,.....,q_oN)------初始关节变量值；

(q_f1,q_f2,......,q_fN)-----终点关节变量值；

N--------是1到6之间的所有正整数；

(3)输入1到6轴在初始点和终点的速度、加速度、初始关节变量值(q_o1,q_o2,.....,q_oN)、终点关节变量值(q_f1,q_f2,......,q_fN)；输入抬起点和下降点的位置(q₁,q₂)以及运动时间T和各轨迹段时间Δτ_i；

(4)利用3－5－3轨迹规划公式计算机械臂1到6轴各轨迹段多项式系数；

(5)计算1到6轴在每一瞬时τ(0＜τ＜T)的位置、速度、加速度；

(6)输出机械臂1到6轴运动规律。

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

Claims (5)

1.一种双机械臂协调控制***，包括控制键盘、主操纵手、显示器、主控制柜、两个从控制柜；

所述机械臂包括结构相同的左臂和右臂，机械臂的机械结构包括基座，基座上安装有支撑臂，支撑臂上端为肩关节，上臂安装在肩关节上，上臂的前端与肘关节连接，肘关节与前臂连接，前臂的末端为腕部；支撑臂的底部安装伺服电机、伺服电机减速器，肩关节和肘关节上设有绝对值编码器；上臂包括依次设置的腰部回转轴、上臂俯仰轴和前臂俯仰轴，提供腰部回转、上臂俯仰和前臂俯仰的运动；前臂包括依次设置的腕部俯仰轴、腕部摇摆轴、腕部旋转轴，提供腕部俯仰、腕部摇摆和腕部旋转的运动；

所述主操纵手与机械臂的机械结构完全相同，主操纵手还包括正交旋转编码器和控制按钮，它主要用于主从式控制方式时，机器人位控信号输入；正交旋转编码器联接在主操纵手的各个关节中，以检测主操纵手各关节位置变化，主操纵手控制按钮用于控制主操纵手位置信号的有效性；

控制键盘的输出接主控制柜的输入，主控制柜的输入输出接显示器的输入输出，主操纵手的输出接主控制柜的输入；

其特征是，主控制柜包括工控机，主控制柜的网口分别与从控制柜的网口通信；所述主控制柜的工控机采集控制键盘的指令，通过网口通信下发给从控制柜的工控机，从控制柜的工控机下发速度指令、伺服驱动指令给运动控制器；

通过采集控制键盘与主操纵手的脉冲数据，以指令形式发送给下位机，从而完成对两个机械臂的运动控制，主控制柜的工控机是连接主操纵手的计算机，通过串口和控制机械臂的从控制柜的工控机进行通讯，发出运动指令，接触编码器信号接口板负责读取各关节的绝对位置信息并且同从控制柜的工控机进行通讯；

从控制柜包括工控机、运动控制器、采集器、稳压电源、伺服电机驱动器，稳压电源分别给工控机、运动控制器、伺服电机驱动器、采集器供电；机械臂的伺服电机分别与伺服电机驱动器、采集器相连；伺服电机驱动器与运动控制器相连，采集器、运动控制器与工控机相连，所述采集器读取绝对位置信息，再通过串口把这些信息传送给从控制柜的工控机；所述运动控制器接收运动控制指令，完成伺服控制功能。

2.如权利要求1所述的双机械臂协调控制***，其特征是，所述主控制柜的工控机及从控制柜的工控机均采用IPC-6806S紧凑型机箱，主板采用PCA-6774主板，1GHz CPU。

3.如权利要求1所述的双机械臂协调控制***，其特征是，所述运动控制器采用PMAC，数字PID控制算法，PID参数在***运行时可实时修改，内部梯形速度轨迹运算器。

4.如权利要求1所述的双机械臂协调控制***，其特征是，所述伺服电机驱动器的输入采用0-10V模式量。

5.如权利要求1所述的双机械臂协调控制***的控制方法，其特征是，包括如下步骤：

(1)输入机械臂末端在初始点和终点的位置和姿势，用位姿矩阵T₁，T₂表示；

-----初始位姿矩阵；

------表示机械臂末端在世界坐标系中的初始位置；

------表示机械臂末端在世界坐标系中的初始姿态；

-----终点位姿矩阵；

------表示机械臂末端在世界坐标系中的终点位置；

------表示机械臂末端在世界坐标系中的终点姿态；

(2)进行运动学反解计算，求得对应于T₁和T₂的两组关节变量值(q_o1,q_o2,.....,q_oN)，(q_f1,q_f2,......,q_fN)；

(q_o1,q_o2,.....,q_oN)------初始关节变量值；

(q_f1,q_f2,......,q_fN)-----终点关节变量值；

N--------是6；

(3)输入1到6轴在初始点和终点的速度、加速度、初始关节变量值(q_o1,q_o2,.....,q_oN)、终点关节变量值(q_f1,q_f2,......,q_fN)；输入抬起点和下降点的位置(q₁,q₂)以及运动时间T和各轨迹段时间Δτ_i；

(4)利用3－5－3轨迹规划公式计算机械臂1到6轴各轨迹段多项式系数；

(5)计算1到6轴在每一瞬时τ(0＜τ＜T)的位置、速度、加速度；

(6)输出机械臂1到6轴运动规律。