CN101327589B - 遥操作平面冗余度机械臂指令输入及构型控制系统及方法 - Google Patents

Abstract

本发明公开一种机械控制技术领域的遥操作平面冗余度机械臂指令输入及构型控制系统及方法。本发明包括冗余度机械臂控制台、冗余度机械臂上位控制器、上位机无线数据模块、下位机无线数据模块、冗余度机械臂下位机控制器以及冗余度机械臂组成，通过机械臂控制台向机械臂发出控制指令，选择单轴运动模式、末端运动模式和构型调整模式中的一种，其中通过单轴运动模式操作人员控制机械臂各轴的运动；通过末端运动模式，操作人员直接控制机械臂末端的运动；通过构型调整模式，操作人员可在保持机械臂末端位置和姿态不变的情况下，调整机械臂的构型，适用于障碍环境，提高了冗余度机械臂在复杂环境下的操作性能。

Description

遥操作平面冗余度机械臂指令输入及构型控制系统及方法

技术领域

本发明涉及的是一种控制技术领域的控制器及控制方法，具体是一种适应多障碍约束空间作业的遥操作平面冗余度机械臂指令输入及构型控制系统及方法。

背景技术

在核工业、反恐排爆、救援、高电压设备清扫、维护等危险、恶劣环境中的检查、维修、搬运、抓取、装配等特种作业广泛采用遥操作机械臂，目前的机械臂自由度大多小于或等于6，适用于无障碍环境；而对于某些特殊的多障碍约束环境作业，如排爆作业中从车底抓取爆炸物，救援作业中从窗户进入靠近地面的被困人员，高压带电清扫变电站中的绝缘子等作业，由于工作环境中存在较多障碍，需要机械臂提供冗余自由度。冗余自由度主要用来扩大机械臂工作范围、优化机械臂运动学、动力学性能以及回避障碍。平面冗余度机械臂是指在一个平面内机械臂自由度大于3的机械臂，是冗余度机械臂的一个重要组成部分。

面向平面冗余度机械手的主端指令输入和控制装置主要包括：按钮、操纵杆、同构主手以及和手臂相捆绑的随动主手等，其中同构主手需要和从手完全相同的机械构型，结构复杂，通常需要位置传感器获取主手各关节位置信息，成本较高；采用和手臂随动的主手则同样存在尺寸较大、结构复杂、造价昂贵的缺点。由于冗余度机械臂除了可以满足操作者对末端位置和姿态的控制要求，还可以进一步调整机械手各关节臂的相对位置关系即构型以适应多障碍环境，而目前以按钮和操纵杆为主的指令输入和控制器尚没有相应的指令输入和处理方法。

经对现有技术的文献检索发现，中国专利ZL200510042571.2研制了一种多关节操作杆，包括基座、尾摆动杆、变向摆动杆、同项摆动杆、头摆动杆、回转杆、按钮依次首尾连接而成，各个摆动和回转关节的中心位置安装中空薄膜电位器和空心轴，按钮开关则装有薄膜开关，用于控制机械臂基座回转、摆动关节1-3、手腕回转和开关关节运动。此操作杆线缆内藏，可根据从手形式组合，结构紧凑；专利200610082456.2则提出一种主从机械手用于外科手术，其中主手 采用与从手同构的连杆串联连接而成，各个连杆驱动时采用双向离合器用来防止操作者向错误方向操作机械臂。

上述机械臂指令输入和控制器，在操作灵活性、安全性等方面有一定提高，但仍存在如下的缺点：(1)控制的对象是非冗余度机械手。(2)控制器仅仅考虑末端的运动，没有专门针对机械臂构型实施控制。而机械臂构型调整则体现了对机械臂对环境的适应能力；(3)结构复杂，造价高。

又发现，中国专利ZL200510042571.2结构紧凑，但是没有考虑机械臂构型的调整方法，实质上是一种集成的单轴运动控制器；专利200610082456.2是一种同构主手，该方案可以用于冗余度机械臂构型控制，但需要设计和从手同构的主手机械结构，并配备了位姿传感器，结构复杂，造价高，仅仅适合于重要的应用场合。

目前大多数机械臂的指令输入和控制器仍然应用通用操作杆(最大控制轴数为3)和按钮组合的方式输入指令，如何在此基础上实现冗余度机械臂末端运动和构型调整尚未见报道。

发明内容

本发明针对现有技术的不足，提供一种冗余自由度小于或等于3的遥操作平面冗余度机械臂指令输入及构型控制系统及方法，使得既可以直接控制机械臂的末端位姿，又可以障碍环境中调整机械臂的构型，从而可实现在障碍环境中快速、高效接近目标，简单实用，提高了平面冗余度机械臂的操作性能。

本发明是通过以下技术方案实现的：

本发明所涉及的遥操作平面冗余度机械臂指令输入及构型控制系统，包括冗余度机械臂控制台、冗余度机械臂上位控制器、上位机无线数据模块、下位机无线数据模块、冗余度机械臂下位机控制器以及冗余度机械臂组成，其中：

所述冗余度机械臂控制台提供人机交互，负责接收操作员的指令并显示机械臂当前位置、限位、报警等状态信息；

所述冗余度机械臂上位控制器安装在冗余度机械臂控制台上，和冗余度机械臂上位控制器之间通过控制电路连接，冗余度机械臂上位控制器通过串行通讯接口与上位机无线数据模块连接与通讯，将编码的控制命令发送到下位机无线数据模块，并同时获取机械臂的状态信息。

所述上位机和下位机无线数据模块提供冗余度机械臂上位控制器和冗余度机械臂下位机控制器之间的通讯功能，由工作在同一工作频率的上位机无线数据模块和下位机无线数据模块组成，其间的距离为100米。上位机和下位机无线数据模块和冗余度机械臂上、下位控制器之间均通过串行通讯接口交换数据；

所述冗余度机械臂下位机控制器安装在冗余度机械臂上，接收冗余度机械臂上位控制器对冗余度机械臂各关节轴的运动指令，并输出到冗余度机械臂的各关节电机上，同时将冗余度机械臂的位置、速度、限位和报警等状态信息发送给下位机无线数据模块。

所述的冗余度机械臂控制台上设有模式选择开关、单轴指令输入开关组、构型指令输入开关组、单轴速度输入旋钮、末端指令输入操纵杆、指示灯和液晶显示模块。通过模式选择开关可选择机械臂的三种控制模式：单轴运动模式，末端运动模式和构型调整模式，在任一时刻只能选择其中一种控制模式。

在单轴运动模式下，单轴指令输入开关组和单轴速度输入旋钮的输入有效。单轴指令输入开关组控制机械臂每个电机的正反转，单轴速度输入旋钮表示轴电机的运动速度，单轴运动指令由单轴指令输入开关组和单轴速度输入旋钮组合实现。

在末端运动模式下，末端指令输入操纵杆和构型指令输入开关组有效。末端指令输入操纵杆直接控制机械臂末端在平面内的运动速度，操作直观性强，所述的操纵杆为三维操纵杆，内部包含三个独立的电位计，可输入三个轴的速度指令；构型指令输入开关组包括1-3个冗余轴选择波段开关，对应于平面冗余度为1到3的情况。在此模式下构型指令输入开关组所选择的轴表示该轴电机锁定。

在构型调整模式下，末端指令输入操纵杆和构型指令输入开关组有效。构型调整模式指的是机械臂末端位置和姿态均保持不变的情况下，通过输入冗余关节的运动，调整机械臂各关节之间的相对位置关系。该功能是这样实现的：由构型指令输入开关组选择要调整的关节轴，此时末端指令输入操纵杆三个电 位计的输入表示由构型指令输入开关组设定的关节轴的运动速度。

所述的指示灯，主要功能为显示各关节轴的限位信息以及电机运动过程的报警信息。

所述的液晶显示模块，主要功能为显示关节轴电机的位置及报警的详细信息。

所述的冗余度机械臂上位控制器，主要由数字量输入接口、滤波电路、模拟量输入接口、数字量输出扩展模块、数字量输出接口、主控芯片、数据存储模块和串行通讯接口组成。其中，主控芯片通过通用I/O端口与数字量输入接口、数字量输出扩展模块连接，其中数字量输入接口用于接入模式选择开关、单轴指令输入开关组以及构型指令输入开关组的开、合指令输入；数字量输出扩展模块用于扩展输出端口，经数字量输出接口完成光电隔离、电平转换后与液晶显示模块以及指示灯连接；主控芯片通过A/D端口和模拟量输入接口连接，经滤波电路后采集冗余度机械臂控制台的单轴速度控制旋钮和末端指令输入操纵杆输入的电压量；主控芯片通过I²C总线与数据存储模块连接。数据存储模块用于存储上位机命令解析程序、机械臂系统的工作参数、远程通讯地址和通讯协议，其存储的数据掉电不丢失。

所述的冗余度机械臂下位机控制器，与机械臂固定安装，包括串行通讯接口、数字信号处理器、高速脉冲输入接口、数字量输入接口、数字量输出接口以及模拟量输出接口。其中，数字信号处理器与高速脉冲输入接口之间通过数据总线连接，用于接收电机的编码器信号，反馈电机的位置、速度信息；数字信号处理器通过GPIO(通用输入输出)端口和数字量输入接口以及数字量输出接口连接，数字量输入接口用于接入原点、限位以及驱动器状态；数字量输出接口用于接入驱动器控制信号；数字信号处理器通过数据端口和模拟量输出接口连接，用于向驱动器提供速度指令。数字信号处理器通过串行通讯接口与无线数据模块通讯，接收上位控制器的控制指令，并反馈电机的位置、速度以及报警等状态信息。

所述的冗余度机械臂由安装在其关节处的电机进行驱动，电机附带编码器和驱动器。编码器通过高速脉冲输入接口和数字信号处理器连接，提供任意时刻电机的位置和速度信息；驱动器通过模拟量输入接口和数字信号处理器连接， 接收电机运动的速度指令；同时通过数字量输入接口接收数字信号处理器的伺服使能等开关指令，并反馈电机的状态信息。

本发明所涉及的遥操作平面冗余度机械臂指令输入及构型控制方法，包括如下的步骤：

2001：读取无线数据模块发送的机械臂当前位置和状态。通过模式选择按钮，操作人员可选择单轴运动模式，末端运动模式和构型调整模式中的一种；

2002：如果选择单轴运动模式，则读取单轴指令输入开关组和单轴速度控制旋钮的值，表示电机的运动方向和速度指令输入；从而确定机械臂全部关节电机的运动速度指令。以下转2007

2003：如选择末端运动模式，则读取构型指令控制开关组的值，构型指令开关的数目等于冗余自由度数目，通过构型指令输入开关组可选择末端运动时不运动的关节电机；

2004：读取末端指令操纵杆的值，包括X轴电位计、Y轴电位计和旋转电位计的值，X轴电位计、Y轴电位计和旋转电位计的值表示机械臂末端的运动指令，其中X轴电位计表示机械臂末端X轴的平动，Y轴电位计表示机械臂末端Y轴的平动，旋转电位计表示机械臂末端在平面内的转动。

通过2003，2004的步骤，可唯一确定冗余度机械臂的末端运动。并通过反解确定机械臂全部关节电机的运动速度指令。以下转2007

2005：如选择构型调整模式，则读取构型指令输入开关组的值，通过构型指令输入开关组选择的轴表示将要实施构型调整运动的电机。构型调整运动的电机数目等于冗余自由度数。

2006：读取末端指令操纵杆的值，包括X轴电位计、Y轴电位计和旋转电位计的值，X轴电位计、Y轴电位计和旋转电位计的值表示构型调整运动指令。其中X轴、Y轴和旋转电位计表示构型指令输入开关组设定的电机的运动。

通过2005，2006的步骤，可保证机械臂末端位置和姿态不变的情况下调整机械臂的构型。并通过反解运算确定机械臂全部关节电机的运动速度指令。

2007：将机械臂关节电机运动速度指令通过远程通讯发送到下位控制器。如果需要执行新的控制命令，则需要重新重复步骤2001-2007。

步骤2003、2004以及2005、2006中，冗余度机械臂电机速度反解算法如 下：

设平面冗余度机械臂的末端位姿由(x，y，α)表示，其中(x，y)表示在运动平面内机械臂的末端位置，α则表示在运动平面内机械臂的姿态。设平面冗余度机械臂关节数为n，且满足4≤n≤6；各个关节的角度值为θ₁～θ_n，则末端位姿(x，y，α)计算如下：

x＝f_x(θ₁，θ₂，...，θ_n)

y＝f_y(θ₁，θ₂，...，θ_n)，4≤n≤6

α＝f_α(θ₁，θ₂，...，θ_n)

式中，f_x、f_y、f_α分别表示末端位姿和关节运动的函数关系。

末端速度计算如下

$\stackrel{\·}{z}=\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{y}\\ \stackrel{\·}{\mathrm{\α}}\end{array}\right]=\left[\begin{array}{cccc}\frac{{\∂f}_x}{{\∂\mathrm{\θ}}_1}& \frac{{\∂f}_x}{{\∂\mathrm{\θ}}_2}& ...& \frac{{\∂f}_x}{{\∂\mathrm{\θ}}_n}\\ \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_1}& \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_2}& ...& \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_n}\\ \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_1}& \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_2}& ...& \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_n}\end{array}\right]\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\\ .\\ .\\ .\\ {\stackrel{\·}{\mathrm{\θ}}}_n\end{array}\right]=J\stackrel{\·}{\mathrm{\Θ}}$

步骤2003和2004中，设通过构型指令输入开关组选择的轴轴数为n-3，编号为第4～n轴，4≤n≤6。如果n＝4，则表示通过构型指令开关组选择第4轴；如果n＝5，则表示通过构型指令开关组选择第4、5轴；如果n＝6，则表示通过构型指令开关组选择第4、5、6轴。所选择轴的速度为零。末端速度指令由操纵杆指定，则轴1～3的速度反解如下：

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\\ {\stackrel{\·}{\mathrm{\θ}}}_3\end{array}\right]={\left[\begin{array}{ccc}\frac{{\∂f}_x}{{\∂\mathrm{\θ}}_1}& \frac{{\∂f}_x}{{\∂\mathrm{\θ}}_2}& \frac{{\∂f}_x}{{\∂\mathrm{\θ}}_3}\\ \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_1}& \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_2}& \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_3}\\ \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_1}& \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_2}& \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_3}\end{array}\right]}^{-1}\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{y}\\ \stackrel{\·}{\mathrm{\α}}\end{array}\right]---\left(1\right)$

步骤2005和2006中，设通过构型指令输入开关组选择的轴数为n-3，编号为第4～n轴，4≤n≤6。如果n＝4，则表示通过构型指令开关组选择第4轴；如果n＝5，则表示通过构型指令开关组选择第4、5轴；如果n＝6，则表示通过构型 指令开关选择组选择第4、5、6轴。所选择轴的构型调整速度指令由操纵杆指定，其余1～3轴的速度计算如下：

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\\ {\stackrel{\·}{\mathrm{\θ}}}_3\end{array}\right]=-{\left[\begin{array}{ccc}\frac{{\∂f}_x}{{\∂\mathrm{\θ}}_1}& \frac{{\∂f}_x}{{\∂\mathrm{\θ}}_2}& \frac{{\∂f}_x}{{\∂\mathrm{\θ}}_3}\\ \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_1}& \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_2}& \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_3}\\ \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_1}& \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_2}& \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_3}\end{array}\right]}^{-1}\left[\begin{array}{c}\frac{{\∂f}_x}{{\∂\mathrm{\θ}}_4}\\ \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_4}\\ \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_4}\end{array}\right]{\stackrel{\·}{\mathrm{\θ}}}_4-...-{\left[\begin{array}{ccc}\frac{{\∂f}_x}{{\∂\mathrm{\θ}}_1}& \frac{{\∂f}_x}{{\∂\mathrm{\θ}}_2}& \frac{{\∂f}_x}{{\∂\mathrm{\θ}}_3}\\ \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_1}& \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_2}& \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_3}\\ \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_1}& \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_2}& \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_3}\end{array}\right]}^{-1}\left[\begin{array}{c}\frac{{\∂f}_x}{{\∂\mathrm{\θ}}_n}\\ \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_n}\\ \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_n}\end{array}\right]{\stackrel{\·}{\mathrm{\θ}}}_n---\left(2\right)$

式(1)和式(2)中，θ_i(i＝1，...，n)表示平面冗余度机械臂第i关节的位置， 

表示平面冗余度机械臂第i关节的运动速度。(x，y，α)表示平面冗余度机械臂的末端位姿f_x、f_y、f_α分别表示末端位姿和关节运动的函数关系。

所述的冗余度机械臂，是指在某一平面内自由度数目大于3的冗余度机械臂，对机械臂在该平面内的运动控制可采用单轴运动指令输入、末端运动指令输入和构型调整指令输入三种控制模式；除此之外，该机械臂还可具备多个自由度，在本发明中，这些自由度只能输入单轴运动指令。

本发明针对平面冗余度机械臂，提出了末端运动指令输入和构型控制方法：其中机械臂末端运动指令输入采用三自由度商用操纵杆控制平面冗余度机械臂末端运动，采用构型指令输入开关组选择锁定关节，并通过控制算法反解得到机械臂各关节的运动指令，简单直观，适合于空旷无障碍环境；而基于末端位姿保持的构型指令输入采用通用操纵杆输入待调整构型的轴的运动，并通过控制算法解算得到机械臂构型，该方法适合于障碍环境下机械臂构型的调整。通过这两种指令输入装置和控制算法的组合，使得既可以直接控制机械臂的末端运动，又可以在障碍环境下调整机械臂的构型，简单实用，适用于大多数平面冗余度机械臂的控制指令输入，如采用六维鼠标等空间运动指令输入装置，还可应用于空间冗余度机械臂的控制。

附图说明

图1为本发明系统结构框图；

图2为上位控制器硬件连接框图；

图3为下位控制器硬件连接框图；

图4为机械臂指令输入和构型控制流程图。

图5为实施例中的冗余度机械臂结构示意图。

具体实施方式

下面结合附图对本发明的实施例作详细说明：本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

本实施例以平面四自由度机械臂的指令输入和构型控制为例，对本发明进行详细说明。

如图1所示，遥操作平面冗余度机械臂指令输入及构型控制系统包括冗余度机械臂控制台1、冗余度机械臂上位控制器2、上位机无线数据模块3、下位机无线数据模块4、冗余度机械臂下位机控制器5以及冗余度机械臂6。冗余度上位控制器2和上位机无线数据模块3安装在机械臂控制台1上，与机械臂控制台1通过控制电路连接；下位机无线数据模块4和冗余度机械臂下位机控制器5安装在冗余度机械臂6上，冗余度机械臂上位控制器2与上位机无线数据模块3通过串行通讯接口连接，冗余度机械臂下位机控制器5和下位机无线数据模块4之间也通过串行通讯接口连接。

如图2所示，所述的冗余度机械臂控制台1，包括模式选择开关101、单轴指令输入开关组102、构型指令输入开关组103、单轴速度输入旋钮104、末端指令输入操纵杆105、指示灯106和液晶显示模块107。这些器件和上位机控制器通过控制电路连接。

模式选择开关101为波段开关，可选择机械臂的三种控制模式：单轴运动模式，末端运动模式和构型调整模式。在任一时刻只能选择其中一种控制模式；在单轴运动模式下，单轴指令输入开关组102和单轴速度输入旋钮104的输入有效；单轴指令输入开关组101的作用是控制机械臂每个电机的正反转，单轴速度输入旋钮103表示电机的运动速度，单轴运动指令由单轴指令输入开关组101和单轴速度输入旋钮103组合实现；

在末端运动模式下，末端指令输入操纵杆105和构型指令输入开关组103有效。末端指令输入操纵杆105的功能是直接控制平面冗余度机械臂末端的运动速度，操作直观性强，末端指令输入操纵杆105为三维操纵杆，内部包含三个独立的电位计，可输入三个轴的速度指令；本实施例中选择的操纵杆为日本 Sakae公司的30JHK-ZT-30R3G三维操纵杆；构型指令输入开关组103包括1个冗余轴选择开关，表示可用于构型调整的自由度数为1，设在此模式下构型指令输入开关所选择的轴为1#轴，则表示在末端运动模式下该轴电机锁定。

在构型调整模式下，末端指令输入操纵杆105和构型指令输入开关组103有效。构型调整模式指的是机械臂末端位置和姿态均保持不变的情况下，通过输入冗余关节的运动，调整机械臂各关节之间的相对位置关系。该功能是这样实现的：由构型指令输入开关组103选择要调整的冗余关节，设选择3#轴；此时末端指令输入操纵杆105的三个电位计的输入中，X轴电位计的输入表示3#轴的运动速度，Y轴电位计和旋转电位计的输入无效。

指示灯106，主要功能为显示各关节轴的限位信息以及电机运动过程的报警信息；液晶显示模块107，主要功能为显示关节轴电机的位置及报警的详细信息。

冗余度机械臂上位控制器2，主要由数字量输入接口201、滤波电路202、模拟量输入接口203、数字量输出扩展模块204、数字量输出接口205、主控芯片206、数据存储模块207和串行通讯接口208组成。这些部件均采用常规的数字电路或电子元器件实现。主控芯片可以是ARM、DSP、FPGA等可编程芯片。

本实施例中，主控芯片206为PHILIPS公司的LPC2131芯片，与数字量输入接口201、数字量输出扩展模块205之间通过片上通用I/O端口连接，其中数字量输入接口201用于接入模式选择开关、单轴指令输入开关组以及构型指令输入开关组的开、合指令输入；数字量输出扩展模块205用于扩展输出端口，经数字量输出接口204完成光电隔离、电平转换后与液晶显示模块107以及指示灯106连接；主控芯片206与模拟量输入接口203之间通过A/D端口连接，经滤波电路202后采集冗余度机械臂控制台的单轴速度控制旋钮104和末端指令输入操纵杆105输入的电压量；主控芯片206通过I²C总线与数据存储模块207连接。数据存储模块207用于存储上位机命令解析程序、机械臂系统的工作参数、远程通讯地址和通讯协议，其存储的数据掉电不丢失。

数据存储模块207用于存储上位机命令解析程序、机械臂系统的工作参数、远程通讯地址和通讯协议，其存储的数据掉电不丢失；数据存储模块207采用电可擦除可编程只读存储器EEPROM，EEPROM207与LPC2131通过I²C总线连接。

冗余度机械臂上位控制器2，与上位机无线数据模块3之间通过串行通讯接 口208连接与通讯，将编码的控制命令发送到下位机无线数据模块，并同时获取机械臂6的状态信息。本例的上位机无线数据模块3和下位机无线模块4，选用北京捷麦通信器材有限公司生产的F-49P无线模块。

如图3所示，冗余度机械臂下位机控制器4，与机械臂6固定安装，包括串行通讯接口301，数字信号处理器302、高速脉冲输入接口303、数字量输入接口304、数字量输出接口305以及模拟量输出接口306。其中，数字信号处理器302与无线数据模块之间通过串行通讯接口301通讯，数字信号处理器302与高速脉冲输入接口303之间通过数据总线连接，用于接收电机的编码器信号，反馈电机的位置、速度信息；其中高速脉冲输入接口303的功能包括正交编码信号处理、计数以及地址译码；可以由常规数字电路和分立器件组成，也可由CPLD/FPGA等集成电路组成；数字信号处理器302与数字量输入接口304、数字量输出接口305之间通过GPIO(通用输入输出)端口和连接，其中数字量输入接口304的功能为光电隔离、端口扩展以及电平转换，由常规的数字电路和电子元器件组成；数字量输出接口305的功能为信号放大、端口扩展以及继电器控制，也由常规的数字电路和电子原器件组成。数字量输入接口304用于接入原点、限位以及驱动器状态如报警等信号；数字量输出接口305用于接入驱动器控制信号如驱动器使能以及报警清除等信号；数字信号处理器302与模拟量输出接口306之间通过数据端口连接，模拟量输出接口306的功能包括D/A转换和电压放大，由常规的数字电路和电子元器件组成。模拟量输出端口用于向驱动器提供速度指令。

本实施例中，数字信号处理器302为TI公司的TMS320LF2407A芯片，通过串行通讯接口与无线数据模块通讯，接收上位控制器的控制指令，并反馈电机的位置、速度以及报警等状态信息。与高速脉冲输入接口303、数字量输入接口304、数字量输出接口305以及模拟量输出接口306之间通过扩展端口连接。

本实施例中采用日本松下MINAS-A4系列交流伺服电机和驱动器。其中电机安装在冗余度机械臂的驱动关节处，用于驱动冗余度机械臂的关节运动，并附带驱动器和编码器。编码器和数字信号处理器之间通过高速脉冲输入接口303连接，提供任意时刻电机的位置和速度信息；驱动器与数字信号处理器之间通过模拟量输出接口306连接，接收电机运动的速度指令；同时通过数字量输出 接口304接收数字信号处理器302的伺服使能、报警清除，机械臂各关节的限位、原点和驱动器状态信息如报警等与数字信号处理器302通过数字量输入接口连接并反馈电机的状态信息。

实施例中的冗余度机械臂如图5所示。冗余度机械臂6包括腰部机构501、第一臂502、第一推杆503、第二臂504、第二推杆505、第三臂506、第三推杆507、连杆机构508、手腕机构509、第四推杆510、第四臂511(手抓)。第一臂502的一端采用销钉方式连接腰部机构501，第一推杆503采用销钉方式连接腰部机构501和第一臂502，第一臂502在第一推杆503的伸缩作用下可以实现相对于腰部机构501的俯仰运动，其运动角度范围不小于110度。第二臂504的一端采用销钉方式连接第一臂502的另一端，第二推杆505采用销钉方式连接第一臂502和第二臂504，第二臂504在第二推杆505的伸缩作用下可以实现相对于第一臂502的俯仰运动，其运动角度范围不小于90度。第三臂506的一端采用销钉方式连接第二臂504的另一端，连杆机构508采用销钉方式连接第三臂506和第二臂504，第三推杆507采用销钉方式连接第二臂504和连杆机构508，第三臂506在第三推杆507的伸缩作用和连杆机构508的放大作用下可以实现相对于第二臂504的俯仰运动，其运动角度范围不小于145度。手腕机构509采用销钉方式连接第三臂506的另一端，第四推杆510采用销钉方式连接第三臂506和手腕机构509，手腕机构509在第四推杆510的伸缩作用下可以实现相对于第三臂506的俯仰运动，其运动角度范围不小于90度。手爪511采用轴方式连接到手腕机构509，手爪511为外配装置，可以进行开合动作，在手腕机构509带动下相对于手腕机构9进行回转。其回转角度范围为连续回转，也可以设定为非连续回转的某一角度范围。

在本实施例中，第一、二、三、四臂的构成平面冗余度机械臂，其控制模式包括末端运动控制和构型调整模式；而腰部回转和手腕回转则仅具备单轴运动模式。

如图4所示，本实施例的遥操作平面冗余度机械臂指令输入及构型控制方法，包括如下步骤：

2001：读取下位机无线数据模块4发送的机械臂当前位置和状态。通过模 式选择开关101，操作人员可选择单轴运动模式，末端运动模式和构型调整模式

2002：如果选择单轴运动模式，则读取单轴指令输入开关组102和单轴速度控制旋钮104的值，表示电机的运动方向和速度指令输入；从而确定机械臂各个关节电机的运动速度指令。以下转2007

2003：如选择末端运动模式，则读取构型指令控制开关组103的值，本实施例中，构型指令开关的数目为1，通过构型指令开关组可选择末端运动时锁定的关节电机，设选择第一臂502为锁定关节；

2004：读取末端指令操纵杆的值，包括X轴电位计、Y轴电位计和旋转电位计的值，X轴电位计、Y轴电位计和旋转电位计的值表示机械臂末端的运动指令，其中X轴电位计表示机械臂末端X轴的平动，Y轴电位计表示机械臂末端Y轴的平动，旋转电位计表示机械臂末端在平面内的转动。

通过2003，2004的步骤，可通过式3唯一确定冗余度机械臂的末端运动。并通过反解确定机械臂全部关节电机的运动速度指令。以下转2007

2005：如选择构型调整模式，则读取构型指令开关组103的值，本实施例中，构型指令开关的数目为1，设选择第三臂506进行构型调整运动。

2006：读取末端指令操纵杆的值，本实施例中，仅需读取X轴电位计的值，，表示对第三臂506的构型调整速度指令，Y轴电位计和旋转电位计的输入无效。

通过2005，2006的步骤，可保证机械臂末端位置和姿态不变的情况下调整机械臂的构型。并通过式4反解确定机械臂各个关节电机的运动速度指令。

2007：将机械臂关节电机运动速度指令通过远程通讯发送到下位控制器。如果需要执行新的控制命令，则需要重新重复步骤2001-2007。

步骤2003、2004以及2005、2006中，冗余度机械臂电机速度反解算法如下：

设平面冗余度机械臂的末端位姿由(x，y，α)表示，其中(x，y)表示在运动平面内机械臂的末端位置，α则表示在运动平面内机械臂的姿态。

x＝f_x(θ₁，θ₂，θ₃，θ₄)

y＝f_y(θ₁，θ₂，θ₃，θ₄)

α＝f_α(θ₁，θ₂，θ₃，θ₄)

则末端速度计算如下

$\stackrel{\·}{z}=\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{y}\\ \stackrel{\·}{\mathrm{\α}}\end{array}\right]=\left[\begin{array}{cccc}\frac{{\∂f}_x}{{\∂\mathrm{\θ}}_1}& \frac{{\∂f}_x}{{\∂\mathrm{\θ}}_2}& \frac{{\∂f}_x}{{\∂\mathrm{\θ}}_3}& \frac{{\∂f}_x}{{\∂\mathrm{\θ}}_4}\\ \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_1}& \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_2}& \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_3}& \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_4}\\ \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_1}& \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_2}& \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_3}& \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_4}\end{array}\right]\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\\ {\stackrel{\·}{\mathrm{\θ}}}_3\\ {\stackrel{\·}{\mathrm{\θ}}}_4\end{array}\right]=J\stackrel{\·}{\mathrm{\Θ}}$

步骤2003和2004中，通过构型指令开关组选择第一臂502为锁定关节， 

则轴1～3的速度反解如下：

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_2\\ {\stackrel{\·}{\mathrm{\θ}}}_3\\ {\stackrel{\·}{\mathrm{\θ}}}_4\end{array}\right]={\left[\begin{array}{ccc}\frac{{\∂f}_x}{{\∂\mathrm{\θ}}_2}& \frac{{\∂f}_x}{{\∂\mathrm{\θ}}_3}& \frac{{\∂f}_x}{{\∂\mathrm{\θ}}_4}\\ \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_2}& \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_3}& \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_4}\\ \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_2}& \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_3}& \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_4}\end{array}\right]}^{-1}\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{y}\\ \stackrel{\·}{\mathrm{\α}}\end{array}\right]---\left(3\right)$

步骤2005和2006中，通过构型指令开关组选择的轴为第三臂506，其构型调整速度指令由操纵杆X轴电位计指定，其余1～3轴的速度计算如下：

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\\ {\stackrel{\·}{\mathrm{\θ}}}_3\end{array}\right]=-{\left[\begin{array}{ccc}\frac{{\∂f}_x}{{\∂\mathrm{\θ}}_1}& \frac{{\∂f}_x}{{\∂\mathrm{\θ}}_2}& \frac{{\∂f}_x}{{\∂\mathrm{\θ}}_3}\\ \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_1}& \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_2}& \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_3}\\ \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_1}& \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_2}& \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_3}\end{array}\right]}^{-1}\left[\begin{array}{c}\frac{{\∂f}_x}{{\∂\mathrm{\θ}}_4}\\ \frac{{\∂f}_y}{{\∂\mathrm{\θ}}_4}\\ \frac{{\∂f}_{\mathrm{\α}}}{{\∂\mathrm{\θ}}_4}\end{array}\right]{\stackrel{\·}{\mathrm{\θ}}}_4---\left(4\right)$

本实施例针对平面4自由度机械臂，实现了末端运动指令输入和构型控制，实现方法简单直观，适合于空旷无障碍环境，既可以直接控制机械臂的末端运动，又可以在障碍环境下调整机械臂的构型，简单实用。

Claims (10)

1.一种遥操作平面冗余度机械臂指令输入及构型控制系统，其特征在于：由冗余度机械臂控制台、冗余度机械臂上位控制器、上位机无线数据模块、下位机无线数据模块、冗余度机械臂下位机控制器以及冗余度机械臂组成，其中：

所述冗余度机械臂控制台提供人机交互，负责接收操作员的指令并显示机械臂当前位置、限位、报警状态信息，该冗余度机械臂控制台上设有模式选择开关、单轴指令输入开关组、构型指令输入开关组、单轴速度输入旋钮、末端指令输入操纵杆、指示灯和液晶显示模块，通过模式选择开关选择机械臂的单轴运动模式、末端运动模式和构型调整模式中一种控制模式；

所述冗余度机械臂上位控制器安装在冗余度机械臂控制台上，和冗余度机械臂控制台通过控制电路连接，冗余度机械臂上位控制器通过串行通讯接口与上位机无线数据模块连接与通讯，将编码的控制命令发送到与远端的下位机无线数据模块，并同时获取远端机械臂的状态信息；

所述无线数据模块提供冗余度机械臂上位控制器和冗余度机械臂下位机控制器之间的通讯功能，由工作在同一工作频率的上位机无线数据模块和下位机无线数据模块组成，其间的距离为100米，上位机、下位机无线数据模块和冗余度机械臂上、下位控制器之间均通过串行通讯接口交换数据；

所述冗余度机械臂下位机控制器安装在冗余度机械臂上，接收冗余度机械臂上位控制器对冗余度机械臂各关节轴的运动指令，并输出到冗余度机械臂的各关节电机上，同时将冗余度机械臂的位置、速度、限位和报警状态信息发送给下位机无线数据模块。

2.根据权利要求1所述的遥操作平面冗余度机械臂指令输入及构型控制系统，其特征是，在所述单轴运动模式下，单轴指令输入开关组和单轴速度输入旋钮的输入有效，单轴指令输入开关组控制机械臂每个电机的正反转，单轴速度输入旋钮表示轴电机的运动速度，单轴运动指令由单轴指令输入开关组和单轴速度输入旋钮组合实现。

3.根据权利要求1所述的遥操作平面冗余度机械臂指令输入及构型控制系统，其特征是，在所述末端运动模式下，末端指令输入操纵杆和构型指令输入 开关组有效，末端指令输入操纵杆直接控制机械臂末端在平面内的运动速度，末端指令输入操纵杆为三维操纵杆，内部包含三个独立的电位计，可输入三个轴的速度指令；构型指令输入开关组包括1-3个冗余轴选择波段开关，对应于平面冗余度为1到3的情况，在此模式下构型指令输入开关组所选择的轴表示该轴电机锁定。

4.根据权利要求1所述的遥操作平面冗余度机械臂指令输入及构型控制系统，其特征是，在所述构型调整模式下，末端指令输入操纵杆和构型指令输入开关组有效，由构型指令输入开关组选择要调整的关节轴，此时末端指令输入操纵杆的三个电位计的输入表示由构型指令输入开关组设定的关节轴的运动速度，从而实现机械臂末端位置和姿态均保持不变的情况下，通过输入冗余关节的运动，调整机械臂各关节之间的相对位置关系。

5.根据权利要求1所述的遥操作平面冗余度机械臂指令输入及构型控制系统，其特征是，所述的冗余度机械臂上位控制器，主要由数字量输入接口、滤波电路、模拟量输入接口、数字量输出扩展模块、数字量输出接口、主控芯片、数据存储模块和串行通讯接口组成，其中：主控芯片通过通用I/O端口与数字量输入接口、数字量输出扩展模块连接，数字量输入接口用于接入模式选择开关、单轴指令输入开关组以及构型指令输入开关组的开、合指令输入；数字量输出扩展模块用于扩展输出端口，经数字量输出接口完成光电隔离、电平转换后与液晶显示模块以及指示灯连接；主控芯片通过A/D端口和模拟量输入接口连接，经滤波电路后采集冗余度机械臂控制台的单轴速度输入旋钮和末端指令输入操纵杆输入的电压量；主控芯片通过I2C总线与数据存储模块连接，数据存储模块用于存储上位机命令解析程序、机械臂系统的工作参数、远程通讯地址和通讯协议，其存储的数据掉电不丢失。

6.根据权利要求1所述的遥操作平面冗余度机械臂指令输入及构型控制系统，其特征是，所述的冗余度机械臂下位机控制器，与机械臂固定安装，包括串行通讯接口、数字信号处理器、高速脉冲输入接口、数字量输入接口、数字量输出接口以及模拟量输出接口，其中，数字信号处理器与高速脉冲输入接口之间通过数据总线连接，用于接收电机的编码器信号，反馈电机的位置、速度信息；数字信号处理器与数字量输入接口、数字量输出接口之间通过GPIO端口 连接，数字量输入接口用于接入原点、限位以及驱动器状态；数字量输出接口用于接入驱动器控制信号；数字信号处理器与模拟量输出接口之间通过数据端口连接，模拟量输出接口用于向驱动器提供速度指令，数字信号处理器与无线数据模块之间通过串行通讯接口通讯，串行通讯接口用于接收上位控制器的控制指令，并反馈电机的位置、速度以及报警信息。

7.根据权利要求6所述的遥操作平面冗余度机械臂指令输入及构型控制系统，其特征是，所述的冗余度机械臂由安装在其关节处的电机进行驱动，该电机附带编码器和驱动器，编码器、驱动器与冗余度机械臂下位机控制器的连接如下：编码器与数字信号处理器之间通过所述的高速脉冲输入接口连接，编码器用于提供任意时刻电机的位置和速度信息；驱动器与数字信号处理器之间通过中所述的模拟量输出接口、数字量输入接口连接，模拟量输出接口用于数字信号处理器向驱动器发送电机运动的速度指令，数字量输入接口用于接收数字信号处理器的开关指令，并反馈电机的状态信息。

8.一种遥操作平面冗余度机械臂指令输入及构型控制方法，适用于权利要求1所述的遥操作平面冗余度机械臂指令输入及构型控制系统，其特征在于，包括如下的步骤：

2001：读取无线数据模块发送的机械臂当前位置和状态，通过模式选择按钮，操作人员选择单轴运动模式、末端运动模式和构型调整模式中的一种；

2002：当选择单轴运动模式，则读取单轴指令输入开关组和单轴速度输入旋钮的值，表示电机的运动方向和速度指令输入，从而确定机械臂全部关节电机的运动速度指令，转步骤2007；

2003：如选择末端运动模式，则读取构型指令控制开关组的值，构型指令开关的数目等于冗余自由度数目，通过构型指令输入开关组选择末端运动时不运动的关节电机；

2004：读取末端指令输入操纵杆的值，包括X轴电位计、Y轴电位计和旋转电位计的值，X轴电位计、Y轴电位计和旋转电位计的值表示机械臂末端的运动指令，其中X轴电位计表示机械臂末端X轴的平动，Y轴电位计表示机械臂末端Y轴的平动，旋转电位计表示机械臂末端在平面内的转动；

通过2003，2004的步骤，能唯一确定冗余度机械臂的末端运动，并通过反 解确定机械臂全部关节电机的运动速度指令，转步骤2007；

2005：如选择构型调整模式，则读取构型指令输入开关组的值，通过构型指令输入开关组选择的轴表示将要实施构型调整运动的电机，构型调整运动的电机数目等于冗余自由度数；

2006：读取末端指令输入操纵杆的值，包括X轴电位计、Y轴电位计和旋转电位计的值，X轴电位计、Y轴电位计和旋转电位计的值表示构型调整运动指令，其中X轴、Y轴和旋转电位计表示构型指令输入开关组设定的电机的运动；

通过2005，2006的步骤，保证机械臂末端位置和姿态不变的情况下调整机械臂的构型，并通过反解运算确定机械臂全部关节电机的运动速度指令；

2007：将机械臂关节电机运动速度指令通过远程通讯发送到冗余度机械臂下位机控制器，当需要执行新的控制命令，则重新重复步骤2001-2007。

9.根据权利要求8所述的遥操作平面冗余度机械臂指令输入及构型控制方法，其特征是，所述步骤2003、2004以及2005、2006中，冗余度机械臂电机速度反解运算如下：

设平面冗余度机械臂的末端位姿由(x，y，α)表示，其中(x，y)表示在运动平面内机械臂的末端位置，α则表示在运动平面内机械臂的姿态，设平面冗余度机械臂关节数为n，且满足4≤n≤6；各个关节的角度值为θ₁～θ_n，则末端位姿(x，y，α)计算如下：

x＝f_x(θ₁，θ₂，…，θ_n)

y＝f_y(θ₁，θ₂，…，θ_n)，4≤n≤6

α＝f_α(θ₁，θ₂，…，θ_n)

末端速度计算如下：

式中，f_x、f_y、f_α分别表示末端位姿和关节运动的函数关系。

10.根据权利要求8或9所述的遥操作平面冗余度机械臂指令输入及构型 控制方法，其特征是，所述步骤2003和2004中，设通过构型指令输入开关组选择的轴数为n-3，编号为第4～n轴，4≤n≤6，当n＝4，则表示通过构型指令输入开关组选择第4轴；当n＝5，则表示通过构型指令输入开关组选择第4、5轴；当n＝6，则表示通过构型指令输入开关组选择第4、5、6轴，所选择轴的速度为零，n为平面冗余度机械臂的自由度数，末端速度指令由操纵杆指定，则轴1～3的速度反解如下：

步骤2005和2006中，设通过构型指令输入开关组选择的轴数为n-3，编号为第4～n轴，4≤n≤6，当n＝4，则表示通过构型指令输入开关组选择第4轴；当n＝5，则表示通过构型指令输入开关组选择第4、5轴；当n＝6，则表示通过构型指令输入开关组选择第4、5、6轴，所选择轴的构型调整速度指令由操纵杆指定，其余1～3轴的速度计算如下：

θ_i(i＝1，...，n)表示平面冗余度机械臂第i关节的位置，

表示平面冗余度机械臂第i关节的运动速度，(x，y，α)表示平面冗余度机械臂的末端位姿f_x、f_y、f_α分别表示末端位姿和关节运动的函数关系。 

Images