四自由度折叠式机械手的控制系统
四自由度折叠式机械手的控制系统
摘 要 四自由度折叠式机械手是由一台STD工业控制机管理和控制，通过视觉图像和接近觉传感器操纵机械手在移动空间中完成取送机器人的任务，文中推导了机械手运动学方程，给出了运动学正逆解，详细介绍了可编程位置控制系统和操纵系统。
关键词 爬壁机器人,机械手，可编程位置控制系统
1 引言
此处研制的壁面爬行遥控检查机器人是在恶劣环境中工作，任务是检查核废液对储罐焊缝的腐蚀程度，为此需一机械手将爬行机器人送至罐壁进行检查工作，完成检查任务后，机械手会将机器人从罐壁上抓取回来，机械手安装在运载小车上，运载小车往返于主控站和储罐室。
机械结构和运动学方程
2.1 机械结构
储罐室过道空间很窄，要求机械化搜在保证抓取能力（30kg）条件下，结构要紧凑，因此机械手结构采用折叠式关节型的设计方案（如图1）
人的手臂有肩、肘、腕三个关节，共六个自由度，机械手的设计以模仿人的手臂功能为基础，再根据实际情况进行简化，由于运载小车和爬壁机器人均可在自身运动的平面上移动和转动，所以可省略机械手肩部和腕部的横向运动，故此机械手仅需四个自由度，即大臂和腕部各有一个摆动自由度，小臂有两个自由度，可使其作摆动和悠闲地转动，并且小臂可折回大臂间的空隙，工作时可展开，机械手末端执行器采用分叉式结构。
2.2 运动学方程
机械手工作时主要是通过控制各关节的转角，使手叉按给定的轨迹运动，以此来完成取送机器人的任务，因此必须建立机械手的运动方程。
采用D-H方法，建立各关节的空间坐标系如图2，其中α1，α2，α4顺次为大臂，小臂和手叉的摆角，α3为手臂转角。由4×4齐次坐标变换原理，可得手叉相对基坐标O的位姿矩阵方程：
=
其中为相邻关节之间的坐标变换矩阵，分别为大臂、小臂和手叉的长度缩写符号：，小臂转角是补偿小车相对罐壁平行偏差而设置，其值通常很小，因此上式中，整理后可得
（1）
由此可得手叉位姿与各关节角之间的关系，即机械手运动学正问题解。
由式（1）可得手叉点在基坐标的轨迹方程为
（2）
同理可求得腕关节点在基坐标中的轨迹方程
（3）
机械手对爬壁机器人的作业可分为三种情况：一是机械手张开，将机器人贴附于壁面；二是手叉退出机器人的背孔，恢复初始状态；三是手叉插入背孔，将机器人从壁面上取下，整个作业周期中，机械手手叉中点需通过图2中的ab和bc两段直线，其中ab段使机器人贴于壁面，bc段使手叉插入或退出背孔，即垂直壁面运动和平行壁面运动。综上所述，两种运动均要求线段始终保持垂直壁面的姿态，假设壁面与地面不垂直，即bc线段与基坐标X轴夹角为φ，由此可得
将式（2）和式（3）带入整理可得
（4）
如已知手叉中点的位置，由式子（2）和（4）可进行运动学逆问题求解，得：
（5）
其中，通常壁面与地面垂直，即
，则
图3是根据（1）和（5）式，按给定的各自路径在计算机上仿真出机械手送取机器人的各种工作状态，结果表明，按式（1）和式（5）控制三个关节角可有效地完成取送机器人的任务。
控制系统
由于机械手控制系统将同爬壁机器人的其他子系统一起安装于运载小车上，因此要求控制系统性能可靠，抗干扰性能强，具有积木式结构，可根据调试和遥控操纵要求，灵活组合，选用了STD总线控制机，作为机械手控制系统的主控机。
为确保机械手直线运动的最小位移±1mm,设计了可编程位置控制系统，解决了在单计算机控制系统中，对机械手进行在线运动学插补运算和实时位置运动控制问题，从而简化控制系统，控制系统框图如图4所示，下面先洗介绍可编程控制位置控制和操作系统。
可编程位置控制系统
可编程位置控制系统框图如图5所示，该系统核心部分是可编程精密运动控制模块LM628。主控机通过I/O接口与运动控制模块进行数据交换，方便地对运动控制模块进行编程，其数据经12位数模转换器，经运放输出给速度伺服单元，由光码盘提供位置反馈信号，实现闭环控制系统。
图5中轨迹发生器（梯形速度发生器）和数字PID调节器是运动控制模块的核心，主控机根据机械手运动控制的要求，给出各关节运动参数加速度、最大速度和最终位置及相应的PID参数。轨迹发生器根据三个运动参数，按图6（a） 速度曲线，计算期望轨迹，使控制电机运动速度连续变化，运动控制模块从期望位置减去反馈的实际位置，得到位置误差，经数字PID调节器，自动进行PID运算，调节驱动电机运动运动到期望位置。
任何时刻，若主控机改变某关节的最大速度或最终位置，位置控制系统中的轨迹发生器将相应地调整运动轨迹如图6（b），电机也将随之加速或减速，主控机也可根据位置控制的需要，随时调整PID参数。
由于采用运动控制模块，使位置控制系统一方面自动生成具有连续速度变化的运动轨迹，另一方面根据期望位置和实际位置自动完成PID运算。
3.2 操纵和保护系统
根据机械手的工作特点，设计手动操纵盒，操纵者根据视觉图像和必要的传感器信息，实现遥控操纵，操纵系统由STD5371中断型开关量输入板，STD537光隔式开关量输出板和安装于机器人壳内的接近觉传动器及机器人背孔上端的两个限位开关及操纵盒组成如图7。
机械手接近觉传感器采用PZ-41型光电式距离可调的开关量传感器，操纵机械手使其沿水平或垂直两方向接近壁面，根据四个接近觉传感器的信息，可判定机器人与壁面接触的状态，操纵按键细微调整机器人姿态使其与壁面完全接触。取机器人时，根据视觉图像，操纵机械手接近机器人，并使叉子插入背孔，根据两背孔顶部的限位开关信息，判断机械手插入的状态。
机械手保护系统由各自由度限位开关，中断型开关量输入板，开关量输出板和逻辑电路组成如图7.，开关量输入板具有24路开关量输入和8路中断输入，一旦机械手误操作撞限位开关，其信号输出给开关量输入板，向计算机发出中断申请信号，同时逻辑电路向四电机驱动单元发出外部锁零信号，使电机停止运转，等待中断处理，故障处理结束，主控机通过开关量输出板向故障处理电路发出复位信号，使电机正常工作。除此之外，操纵盒还设置“急停”按键，一旦
保护系统未能阻止机械手误动作，操纵者按动“急停”按键，强迫机械手电机停止工作。
采用上述两种保护措施，基本上避免了由于机械手误动作，造成机械手自身或对爬壁机器人其他子系统的机械损害，保证机械手安全工作。
4 结束语
运行实践证明，由单计算机和可编程位置控制接口板组成的控制系统，不仅设备简单成本低，而且运行可靠，避免了两级计算机控制系统中由于大量信息传递而造成误码，漏码等问题，该控制方式亦可满足一般工业机器人性能要求。
实际操纵表明，我们所设计四自由度折叠机械手，由于结构紧凑，在狭窄空间内完全可以较好的完成机器人的取送任务。
参考文献1 陈佳实，微机控制与微机自适应控制，电子工业出版社，1987.
2 魏庆福等，STD总线工业控制机的设计与应用，科学出版社，1991
3 邓星钟等，机电传动控制，华中科技大学出版社
4 线性代数 同济大学第五版
四自由度折叠式机械手的控制系统.doc
