CN101352854B - 遥操作平面冗余度机械臂自主避障智能单元、系统及方法 - Google Patents

Abstract

一种机械控制技术领域的遥操作平面冗余度机械臂自主避障智能单元、系统及方法。所述系统包括数据存储模块、微处理器、串行通讯接口、现场总线接口以及智能单元，智能单元针对每个轴电机设置，该智能单元采集该关节臂安装的红外距离传感器信息，通过传感信息测量值将关节臂状态分为正常、避障和停止状态：关节臂处于正常状态时，智能单元采用微处理器规划的速度指令控制驱动单元；关节臂处于避障状态时，智能单元采用增大与障碍距离的速度指令驱动控制单元；机械臂处于停止状态时，智能单元停止关节电机运动并将向运动协调模块发送关节臂状态。本发明可简化系统接线，使得当通过遥操作控制机械臂末端趋紧目标的同时可快速感知障碍并实现避障。

Description

遥操作平面冗余度机械臂自主避障智能单元、系统及方法 

技术领域

本发明涉及的是一种机械控制技术领域的控制单元、系统及方法，具体是一种遥操作平面冗余度机械臂自主避障智能单元、系统及方法。 

背景技术

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

通过在机械手的手臂和手爪表面上安装距离传感器，可实时感知机械臂及周边环境信息，并用于避障控制。目前大多通过直接接入或者通过通讯的方式将传感信息接入到机械臂控制器，并通过程序计算得出机械臂和障碍的相对位置关系及避障策略。但是由于传感器种类、数目较多，直接接入控制器将导致控制系统规模庞大，布线复杂，可靠性低，而通过通讯的方式将传感信息接入到机械臂控制器可适度降低系统的复杂度，但集中式信息处理和优化策略仍然会导致系统实时性降低，操作性能下降。 

经对现有技术的文献检索发现，中国专利200710022775.9提出了一种液压挖掘机避障控制系统和方法，通过角度传感器获取挖掘机的动臂、斗杆、铲斗和工作平台的状态，获得挖掘机工作装置的位置参数，并通过手动设置障碍物的预警距离及在线计算判断是否发生碰撞。避障策略包括自动停机和向操作人员发出声音及图像预警。中国专利CN1304178C提出了一种多机械臂机器人关节间的碰撞检测方法，通过在机械臂各关节两端安装传感器，得到机械臂各关节端点的空 间坐标，传递到计算机控制系统后通过程序计算判断两关节在空间是否发生碰撞，从而实现多机械臂关节间的碰撞检测。中国专利CN1861330A提出了一种移动机器人的障碍感知及自主避障方法。采用超声和红外距离传感器感知前部的障碍，并将传感信号传递到微处理器，避障策略则包括慢行、左转、右转、停止等。 

上述技术在机械臂姿态检测、碰撞检测等方面有一定提高，但仍存在如下缺点：(1)碰撞需要通过控制器计算获得机械臂和环境之间的位姿信息，对计算机的计算能力以及多种传感信息集成能力要求高；(2)避障策略需要通过机械臂控制器对外部所有的传感信息获取后进行计算，信息传递过程复杂；(3)避障算法简单，不能适用于冗余度机械臂。专利200710022775.9没有直接的障碍检测器件，需要实时计算挖掘机和障碍的距离，仅适用于简单的结构化障碍分布；专利CN1304178C则需要采集多个障碍感知传感器的信息并集成到控制器进行计算，系统构成复杂，不适合特种机器人快速高效作业的要求；专利CN1861330A也是采用传感信息集中处理，且仅面向移动机器人。 

目前特种作业机器人主要以遥操作为主，而如何在遥操作的框架下实现冗余度机械臂高效自主避障，则未见报道。 

发明内容

本发明针对现有技术的不足，提供了一种遥操作平面冗余度机械臂自主避障智能单元、系统及方法。通过在机械臂及手爪表面设置红外距离传感器来实时感知障碍，并针对每个机械臂关节设置智能单元，该智能单元采集该节机械臂表面红外距离传感器的障碍感知信号，通过传感信息测量值将关节臂状态分为正常、避障和停止状态，并启动不同的运动控制策略，通过该避障方法及智能单元，可以简化系统接线，使得当通过遥操作控制机械臂末端趋近目标的同时可快速感知障碍并实现避障，从而达到高实时性、高可靠性，提高冗余度机械臂在障碍环境下的作业效率和操作性能。 

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

本发明所涉及的遥操作平面冗余度机械臂自主避障智能单元，包括现场总线接口、轴运动数据存储模块、数字信号处理器、运动控制与避障模块、高速脉冲输入接口、模拟量输出接口、数字量输出接口、数字量输入接口、模拟输入接口、驱动单元、上表面红外距离传感器阵列、下表面红外距离传感器阵列，其中： 

数字信号处理器通过地址及数据总线与现场总线接口连接，用于接收微处理器传送的轴运动命令，并反馈轴运动指令及位置、速度及报警等状态信息； 

数字信号处理器通过数据总线和高速脉冲输入接口电路连接，用于接收电机的编码器信号，反馈电机的位置、速度信息； 

数字信号处理器通过数据端口和模拟量输出接口连接，用于向驱动器提供速度指令； 

数字信号处理器通过GPIO(通用输入输出)端口和数字量输入接口以及数字量输出接口连接，数字量输入接口用于接入原点、限位以及驱动器状态；数字量输出接口用于发送驱动器控制信号； 

数字信号处理器通过数据端口与模拟量输入接口连接，用于采集红外距离传感器感知的与环境的距离信息，模拟量输入接口实现信号放大、光电隔离、滤波及A/D转换； 

所述的轴运动数据存储模块，通过I²C总线与数字信号处理器连接，用于存储关节轴工作参数和避障策略； 

所述的驱动单元，用于驱动冗余度机械臂的关节运动，提供任意时刻电机的位置和速度信息，并接收数字信号处理器电机运动的速度指令； 

所述的上、下表面红外距离传感器阵列，感应方向垂直于关节臂外表面。红外距离传感器通过红外发送器以及位置感应器件(PSD)获取与环境的距离，并以模拟量的形式体现，如电压；并通过模拟量输入接口接入数字信号处理器； 

所述运动控制及避障模块运行于数字信号处理器中，用于综合避障系统微处理器的轴运动命令和避障策略，运动控制及避障模块计算轴的运动速度，并根据轴运动数据存储模块中设置的碰撞预警阈值将关节轴分为三种运动状态，正常状态、避障状态以及停止状态，同时还将计算的轴运动速度通过数字信号处理器的模拟量输出端口发送到驱动单元的驱动器，并通过数字信号处理器的高速脉冲接口反馈电机转速，构成速度闭环，从而实现轴的运动。 

所述运动控制及避障模块计算轴的运动速度，轴运动首先在轴运动数据存储模块中设置碰撞预警阈值1和碰撞预警阈值2，阈值1＞阈值2，并存储于轴运动数据存储模块中。运动控制及避障模块实时采集该关节臂上表面红外距离传感器阵列和下表面红外距离传感器阵列，并根据红外距离传感器阵列的输出值，将关 节轴分为三种运动状态：(1)正常状态。此时该关节臂上、下表面红外距离传感器阵列中全部红外距离传感器的测量值均大于碰撞预警阈值1；表明没有碰撞危险，智能单元执行微处理器运动协调模块通过总线发送的规划运动；(2)避障状态。此时上、下表面红外距离传感器阵列中至少有一个红外距离传感器测量值在阈值1和阈值2之间，智能单元将根据存储在轴运动数据存储模块中的避障策略执行避障运动；(3)停止状态。此时上、下表面红外距离传感器阵列中的有至少一个红外距离传感器测量值小于碰撞预警阈值2，表明该关节臂有碰撞危险，智能单元将停止轴运动，并将状态通过总线发送到微处理器的运动协调模块进行处理。 

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

所述上、下表面红外距离传感器阵列，分别由安装在平面冗余度机械臂各关节臂的上下表面的一组红外距离传感器组成，每组红外距离传感器的数目在1到8之间。 

本发明所涉及的遥操作平面冗余度机械臂自主避障系统，包括系统数据存储模块、微处理器、串行通讯接口、现场总线接口以及智能单元1～n。n为平面冗余度机械臂关节数。其中数据存储模块、微处理器、串行通讯接口、现场总线接口安装在冗余度机械臂上，智能单元1～n安装在各关节臂内部。 

所述系统数据存储模块用于存储机械臂运动控制程序、机械臂系统的工作参数、远程通讯地址和通讯协议，其存储的数据掉电不丢失。数据存储模块与微处理器通过I²C总线连接。 

所述的微处理器用于综合远程操作人员提供的机械臂末端运动指令以及智能单元返回的状态信息，通过运动协调模块运算得到机械臂各轴的运动指令； 

所述运动协调模块运行于微处理器中，根据操作人员发送的机械臂末端速度指令和各智能单元发送的各关节状态和速度实施运动规划，并通过现场总线发送到各智能单元。当所有智能单元都处于正常状态时，运动协调模块根据能量最小 原则反解各轴运动速度；当有智能单元处于避障状态且数目小于等于冗余自由度时，通过运动协调模块可以通过协调其他智能单元的运动，从而保证机械臂末端按照操作人员发送的末端速度指令运动的同时实施避障；当有智能单元处于停止状态时，表明该关节臂处于碰撞危险状态，运动协调模块对各轴智能单元发送运动停止命令，停止机械臂运动。 

所述的串行通讯接口，主要用于接收远程的机械臂末端运动指令，并转换为微处理器可以处理的数据格式；同时将机械臂各轴的位置、速度及状态信息发送给远程操作人员；微处理器通过其通用异步收发器(UART)端口与串行通讯接口连接。 

所述的现场总线接口，用于将微处理器计算的各轴运动指令转化为总线数据格式，并通过总线通讯协议发送到各个智能单元，包括总线收发器、光电隔离、电平转换及滤波电路，由常规的数字电路和电子元器件组成。微处理器通过地址及数据总线与总线接口连接。 

所述的智能单元，安装于各关节臂的内部，用于综合微处理器的轴运动命令和红外距离传感器感知的信号，通过运动控制与避障模块运算得到该轴的运动指令，并控制驱动单元完成轴的驱动。智能单元通过总线接口与总线连接。 

本发明所涉及的遥操作平面冗余度机械臂自主避障方法，包括运动控制及避障模块进行的运动控制及避障，以及运动协调模块进行的运动协调两部分，其中： 

所述的运动控制及避障模块进行的运动控制及避障包括如下步骤： 

60401：读取微处理器通过总线发送的轴运动速度指令； 

60402：读取关节臂上的红外距离传感器阵列的输出。该值是经过模数转换后的整型数。 

60403：若有任一红外距离传感器的值＜碰撞预警阈值2，表明机械臂处于危险区域内，需要立刻使系统停止，处理内容转60409；否则继续。 

60404：若上表面红外距离传感器阵列中有任一传感器的值＜碰撞预警阈值1，则表明机械臂处于避障区域，以下转60406，否则转60405。 

60405：若下表面红外距离传感器阵列中有任一传感器的值＜碰撞预警阈值1，则表明机械臂处于避障区域，以下转60407，否则转60408。 

60406：令驱动关节轴电机反向匀速运动进行避障。(设机械臂向上运动为正 向)，同时设关节轴状态为“避障”，以下转60410； 

60407：令驱动关节轴电机正向匀速运动进行避障，同时设关节轴状态为“避障”。以下转60410； 

60408：若所有红外距离传感器输出均大于碰撞预警阈值1，表明机械臂行进过程无障碍，关节轴以规划速度运动，同时设关节轴状态为“正常”。以下转60410； 

60409：令关节轴电机停止运动，同时设关节轴状态为“停止”； 

60410：将关节轴运动输出到驱动单元，同时将电机位置、速度和状态信息发送到微处理器。 

所述运动协调模块进行的运动协调包括如下步骤： 

3001：读取串行通讯接口接收的机械臂末端速度指令。该指令由操作人员远程发送； 

3002：读取智能单元发送的各关节轴位置、状态及控制指令。 

3003：如果有关节轴处于“停止”状态，则表明有关节臂有碰撞危险，已经停止运动，以下转3009，否则转3004。 

3004：如果有关节轴处于“避障”状态，则表明有关节智能单元正在执行避障指令，需要重新规划各关节轴电机的运动；以下转3005，否则表示没有智能单元没有检测到障碍，以下转3006。 

3005：判断处于“避障”状态的关节轴的数目是否小于等于冗余自由度数，如果“是”，这表明可以实施避障运动规划，以下转3007；否则表明无法协调各关节轴运动同时满足机械臂末端运动指令和避障运动指令，转3008。 

3006：按照操作人员的末端速度指令反解得到各轴速度指令； 

通过步骤3005，可通过反解计算得到正常状态下机械臂各关节轴电机的规划速度； 

3007：根据各关节的位置和智能单元的避障运动速度以及操作人员发送的机械臂末端速度指令，重新反解机械臂各轴速度指令； 

通过步骤3007，微处理器可以协调各智能单元的避障策略，通过反解计算机械臂其他各关节轴电机的规划速度，从而保证机械臂末端在执行操作人员速度指令的同时实现关节臂避障运动。 

3008：将机械臂各轴电机速度指令置为零。 

3009：通过总线将各关节轴速度指令发送至各智能单元； 

3010：将各轴位置及状态通过串行通讯接口发送给操作人员。 

步骤3006，3007中，冗余度机械臂电机速度反解算法如下： 

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

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

y＝f_y(θ₁，θ₂，…，θ_n) 

α＝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{\Θ}}$

步骤3006，根据最小能量法选择运动轴为(n-2)～n，n为平面冗余度机械臂的自由度数，第n关节为平面冗余度机械臂最前端关节。则正常状态下机械臂第(n-2)到第n关节电机的运动速度反解如下： 

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

第1～(n-3)关节电机速度为零。 

步骤3007中，设有m个智能单元处于避障状态，m＜n-2，n为冗余度机械臂自由度数，不失一般性，设第(n-3)轴和第(n-2)轴处于避障状态，智能单元的避障指令为 

和 

机械臂末端速度指令为 

则机械臂第(n-4)轴、第(n-1)轴、第n轴的速度计算如下： 

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

其余关节轴电机速度为零。 

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

表示平面冗余度机械臂第i关节的运动速度。 

本发明针对平面冗余度机械臂，根据需要，在各关节臂表面安装多个红外距离传感器，并针对每个轴电机设置智能单元，该智能单元采集该关节臂安装的红外距离传感器信息，通过传感信息测量值将关节臂状态分为正常、避障和停止状态，并启动不同的运动控制策略：(1)当关节臂处于正常状态时，智能单元采用微处理器规划的速度指令控制驱动单元；(2)当关节臂处于避障状态时，智能单元采用增大与障碍距离的速度指令驱动控制单元，这是一种直观的反应式避障策略，简单实用，实时性和可靠性高。(3)当机械臂处于停止状态时，智能单元停止关节电机运动并将向运动协调模块发送关节臂状态。通过本发明提出的自主避障策略和分布式智能单元，使得冗余度机械臂在执行操作人员末端速度指令的同时回避环境中的障碍，提高了工作效率及机械臂在障碍环境的操作性能和安全性，适用于大多数平面冗余度机械臂的避障控制。如在关节臂各个表面均安装红外距离传感器，还可以适用于空间冗余度机械臂的自主避障。 

附图说明

图1为本发明遥操作平面冗余度机械臂自主避障系统结构框图； 

图2为遥操作平面冗余度机械臂自主避障智能单元硬件结构框图； 

图3为智能单元运动控制及避障模块进行运动控制及避障的流程图； 

图4为微处理器运动协调模块进行运动协调的流程图； 

图5为手爪的红外距离传感器阵列示意图。 

具体实施方式

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

本实施例针对遥操作平面四自由度机械臂自主避障方法和智能单元，对本发明进行详细说明。 

如图1所示，本实施例所述遥操作平面冗余度机械臂运动控制与避障系统，包括系统数据存储模块1、微处理器2、运动协调模块3、串行通讯接口4、现场总线接口5以及1-4#智能单元6。其中系统数据存储模块1、微处理器2、串行通讯接口4、现场总线接口5安装在冗余度机械臂上，4个智能单元5安装在各关节臂内部。运动协调模块3运行于微处理器2中；微处理器2与系统数据存储模块1通过I²C总线连接，与串行通讯接口3通过通用异步收发器(UART)端口连接，与总线接口通过地址及数据总线连接。 

所述系统数据存储模块1用于存储机械臂运动协调控制程序、机械臂系统的工作参数、远程通讯地址和通讯协议，其存储的数据掉电不丢失。本实施例中，数据存储模块2采用电可擦除可编程只读存储器EEPROM。 

微处理器2用于综合远程操作人员提供的机械臂末端运动指令以及智能单元返回的状态信息，通过运动协调模块运算得到机械臂各轴的运动指令；微处理器可以是ARM、DSP、FPGA等可编程芯片，本实施例中，微处理器2为PHILIPS公司的LPC2294芯片。LPC2294通过I²C总线与电可擦除可编程只读存储器EEPROM连接。 

运动协调模块3运行于微处理器2中，其主要功能为根据操作人员发送的机械臂末端速度指令和各智能单元发送的各关节状态和速度实施运动规划，并通过现场总线发送到各智能单元：当所有智能单元都处于正常状态时，运动协调模块根据能量最小原则反解各轴运动速度；当有智能单元处于避障状态且数目小于等于1时，通过协调模块可以通过协调其他智能单元的运动，从而保证机械臂末端按照操作人员发送的末端速度指令运动的同时实施避障；当有智能单元处于停止状态时，表明该关节臂处于碰撞危险状态，运动协调模块对各轴智能单元发送运动停止命令，停止机械臂运动。 

串行通讯接口4，主要用于接收远程的机械臂末端运动指令，并转换为微处理器可以处理的数据格式；同时将机械臂各轴的位置、速度及状态信息发送给远程操作人员；串行通讯接口可以是RS232，RS422以及RS485接口，本实施例中采用RS485通讯接口，微处理器2通过其通用异步收发器(UART)端口与RS485 串行通讯接口3连接。 

现场总线接口5，用于将微处理器计算的各轴运动指令转化为总线数据格式，并通过总线通讯协议发送到各个智能单元。本实施例中，现场总线接口5采用CAN总线接口，微处理器2通过其地址和数据端口与CAN总线接口5连接。 

智能单元6，包括现场总线接口601，轴运动数据存储模块602、数字信号处理器603、运动控制及避障模块604、高速脉冲输入接口605、模拟量输出接口606、数字量输出接口607、数字量输入接口608模拟输入接口609、驱动单元610以及上表面红外距离传感器阵列611和下表面红外距离传感器阵列612。其中数字信号处理器603通过地址及数据总线与现场总线接口601连接，用于接收微处理器传送的轴运动命令，并反馈轴运动指令及位置、速度及报警等状态信息；运动控制与避障模块604运行数字信号处理器603中，数字信号处理器603通过数据总线和高速脉冲接口电路605连接，用于接收驱动单元610的电机编码器信号，反馈电机的位置、速度信息；数字信号处理器503通过数据端口和模拟量输出接口505连接，用于向驱动单元509的驱动器提供速度指令；数字信号处理器503通过GPIO(通用输入输出)端口和数字量输入接口507以及数字量输出接口506连接，数字量输入接口507用于接入原点、限位以及驱动器状态；数字量输出接口506用于发送驱动器控制信号；数字信号处理器503通过数据端口与模拟量输入接口508连接，用于采集上表面红外距离传感器阵列510和下表面红外距离传感器阵列511感知的与环境的距离信息。模拟量输入接口508包括信号放大、光电隔离、滤波及A/D转换，由常规的数字电路和电子元器件组成。 

本实施例中，数字信号处理器503为TI公司的TMS320LF2407A芯片，通过现场总线接口与微处理器通讯，接收上位控制器的控制指令，并反馈电机的位置、速度以及关节臂正常、避障、停止等状态信息。并通过扩展端口与高速脉冲输入接口504、数字量输入接口507、数字量输出接口506、模拟量输出接口505以及模拟量输入端口508连接。 

运动协调模块3运行于微处理器2上，其主要功能为根据操作人员发送的机械臂末端速度指令和各智能单元发送的各关节状态和速度实施运动规划，并通过现场总线接口5发送到1-4#智能单元6：当所有智能单元都处于正常状态时，运动协调模块根据能量最小原则反解各轴运动速度；当1-4#智能单元中有1 个处于避障状态时，通过运动协调模块可以通过协调其他智能单元的运动，从而保证机械臂末端按照操作人员发送的末端速度指令运动的同时实施避障；当有智能单元处于停止状态时，表明该关节臂处于碰撞危险状态，运动协调模块3通过现场总线接口5对1-4#智能单元发送运动停止命令，停止机械臂运动。 

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

轴运动数据存储模块602，通过I²C总线与数字信号处理器503连接，用于存储关节轴工作参数和避障策略。本实施例中，数据存储模块602采用电可擦除可编程只读存储器EEPROM。 

上表面红外距离传感器阵列611和下表面红外距离传感器阵列612，分别由安装在平面冗余度机械臂各关节臂的上下表面的一组红外距离传感器组成，感应方向垂直于关节臂外表面。本实施例中采用日本Sharp公司的GP2D12红外距离传感器，感应范围10cm-80cm，输出形式为模拟电压量。 

本实施例中对象为平面4自由度机械臂，冗余度为1。在第3、4关节安装了红外距离传感器阵列。其中第4关节即手爪的红外距离传感器阵列如图5所示。其中第四关节上表面和下表面各安装有一个红外距离传感器，该关节臂通过俯仰运动进行避障。 

在第4关节还在左右侧面安装有4个红外距离传感器，该红外距离传感器也接入智能单元，但只起防止碰撞的作用。 

如图3所示，本实施例的智能单元中设置阈值1为20cm，阈值2为11cm。 

本实施例遥操作平面冗余度机械臂自主避障方法具体如下： 

运动控制与避障流程如下： 

60401：读取微处理器通过总线发送的轴运动速度指令； 

60402：读取关节臂上的红外距离传感器阵列的输出。该值是经过模数转换后的整型数。 

60403：若有任一红外距离传感器的值＜碰撞预警阈值2，表明机械臂处于危险区域内，需要立刻使系统停止，处理内容转60409；否则继续。 

60404：若上表面红外距离传感器阵列中有任一传感器的值＜碰撞预警阈值1，则表明机械臂处于避障区域，以下转60406，否则转60405。 

60405：若下表面红外距离传感器阵列中有任一传感器的值＜碰撞预警阈值1，则表明机械臂处于避障区域，以下转60407，否则转60408。 

60406：令驱动关节轴电机反向匀速运动进行避障。(设机械臂向上运动为正向)，同时设关节轴状态为“避障”，以下转60410； 

60407：令驱动关节轴电机正向匀速运动进行避障，同时设关节轴状态为“避障”。以下转60410； 

60408：若所有红外距离传感器输出均大于碰撞预警阈值1，表明机械臂行进过程无障碍，关节轴以规划速度运动，同时设关节轴状态为“正常”。以下转60410； 

60409：令关节轴电机停止运动，同时设关节轴状态为“停止”； 

60410：将关节轴运动输出到驱动单元，同时将电机位置、速度和状态信息发送到微处理器。 

运动协调流程如下： 

3001：读取串行通讯接口接收的机械臂末端速度指令。该指令由操作人员远程发送； 

3002：读取智能单元发送的各关节轴位置、状态及控制指令。 

3003：如果有关节轴处于“停止”状态，则表明有关节臂有碰撞危险，已经停止运动，以下转3009，否则转3004。 

3004：如果有关节轴处于“避障”状态，则表明有关节智能单元正在执行避障指令，需要重新规划各关节轴电机的运动；以下转3005，否则表示没有智能单元没有检测到障碍，以下转3006。 

3005：判断处于“避障”状态的关节轴的数目是否小于等于1，如果“是”，这表明可以实施避障运动规划，以下转3007；否则表明无法协调各关节轴运动 同时满足机械臂末端运动指令和避障运动指令，转3008。 

3006：按照操作人员的末端速度指令反解得到各轴速度指令； 

通过步骤3005，可通过反解计算得到正常状态下机械臂各关节轴电机的规划速度； 

3007：根据各关节的位置和智能单元的避障运动速度以及操作人员发送的机械臂末端速度指令，重新反解机械臂各轴速度指令； 

通过步骤3007，微处理器可以协调各智能单元的避障策略，通过反解计算机械臂其他各关节轴电机的规划速度，从而保证机械臂末端在执行操作人员速度指令的同时实现关节臂避障运动。 

3008：将机械臂各轴电机速度指令置为零。 

3009：通过总线将各关节轴速度指令发送至各智能单元； 

3010：将各轴位置及状态通过串行通讯接口发送给操作人员； 

步骤300，3007中，冗余度机械臂各轴电机速度反解算法如下： 

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

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

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

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

式中，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{\θ}}_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{\Θ}}$

步骤2006，根据最小能量法选择运动轴为2～4，第4关节为平面冗余度机械臂最前端关节。则正常状态下机械臂第2到第4关节电机的运动速度反解如下： 

$\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(1\right)$

第1关节电机速度为零。 

步骤2007中，最多只能有1个智能单元处于避障状态，设为第4关节轴处于避障状态，智能单元的避障指令为 

机械臂末端速度指令为 

则机械臂第1轴、第2轴、第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[\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(2\right)$

其余关节轴电机速度为零。 

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

表示平面冗余度机械臂第i关节的运动速度。 

Claims (2)

1.一种遥操作平面冗余度机械臂自主避障智能单元，其特征在于，包括现场总线接口、轴运动数据存储模块、数字信号处理器、运动控制及避障模块、高速脉冲输入接口、模拟量输出接口、数字量输出接口、数字量输入接口、模拟量输入接口、驱动单元、上表面红外距离传感器阵列、下表面红外距离传感器阵列，其中：

数字信号处理器与现场总线接口之间通过地址及数据总线连接，用于接收微处理器传送的轴运动命令，并反馈轴运动指令及位置、速度及报警状态信息；

数字信号处理器和高速脉冲输入接口电路之间通过数据总线连接，用于接收电机的编码器信号，反馈电机的位置、速度信息；

数字信号处理器与模拟量输出接口之间通过数据端口连接，用于向驱动器提供速度指令；

数字信号处理器与数字量输入接口、数字量输出接口之间通过GPIO端口连接，数字量输入接口用于接入原点、限位以及驱动器状态，数字量输出接口用于发送驱动器控制信号；

数字信号处理器与模拟量输入接口之间通过数据端口连接，用于采集上表面红外距离传感器阵列和下表面红外距离传感器阵列感知的与环境的距离信息，模拟量输入接口实现信号放大、光电隔离、滤波及A/D转换；

所述的轴运动数据存储模块，通过I²C总线与数字信号处理器连接，用于存储关节轴工作参数和避障策略；

所述的驱动单元，用于驱动冗余度机械臂的关节运动，提供任意时刻电机的位置和速度信息，并接收数字信号处理器电机运动的速度指令；

所述的上表面红外距离传感器阵列和下表面红外距离传感器阵列，感应方向垂直于关节臂外表面，红外距离传感器通过红外发送器以及位置感应器件获取与环境的距离，并以模拟量的形式体现，并通过模拟量输入接口接入数字信号处理器；

所述运动控制及避障模块运行于数字信号处理器中，用于综合避障系统微处理器的轴运动命令和避障策略，运动控制及避障模块计算轴的运动速度，并根据轴运动数据存储模块中设置的碰撞预警阈值将关节轴分为三种运动状态，正常状态、避障状态以及停止状态，同时还将计算的轴运动速度通过数字信号处理器的模拟量输出端口发送到驱动单元的驱动器，并通过数字信号处理器的高速脉冲接口反馈电机转速，构成速度闭环，从而实现轴的运动。

2.根据权利要求1所述的遥操作平面冗余度机械臂自主避障智能单元，其特征是，所述的驱动单元，包括安装在冗余度机械臂的驱动关节处的电机，用于驱动冗余度机械臂的关节运动，并附带编码器和驱动器，编码器通过高速脉冲输入接口和数字信号处理器连接，提供任意时刻电机的位置和速度信息；驱动器通过模拟量输入接口和数字信号处理器连接，接收电机运动的速度指令，同时通过数字量输入接口接收数字信号处理器的伺服使能开关指令，并反馈电机的状态信息。

Images