六自由度上料机械手控制系统的设计与测试

高红红，马保吉，杨 恒，李志超

(西安工业大学机电工程学院，陕西西安 710032)

0 引言

随着计算机技术的快速发展，机器人技术应用到多领域。机器人是一种仿人操作、自动控制、可重复编程、能在三维空间完成各种作业的机电一体化设备[1]，可以代替人类在一些恶劣、危险的环境中工作[2]，例如：焊接和自动喷漆，处理爆炸物和放射性物质等。在工业生产中，机械手常用于批量生产和重复性工作场合，进行零件的搬运、分拣和装配等，从而降低生产成本和工人的劳动强度，减少安全事故[3]。高精度的机械臂也可以应用于一些控制较精密的行业，如精密装配等[4]，减小人工随机误差,提高工作精度和产品质量。机械臂的灵活性与其自由度密切相关，多自由度机械臂要实现空间内物体的准确抓取，很大程度上取决于控制系统的控制精度[5]。陈杨洋等设计了基于自定义总线控制的机械臂控制系统，采用主从式控制实现了三自由度机械臂的控制，并利用三坐标测量仪对机械臂末端的三维坐标进行了测量，结果表明机械臂的末端误差在4 mm以内[6]。刘牧歌对五自由度搬运机械手关键技术进行了研究，通过PLC控制伺服电机驱动机械手动作，手腕回转的点动控制精度为36°[7]。龙腾设计了一种以STM32F4为主控制器、陀螺仪传感器为关节状态检测器，由舵机驱动的六自由度机械臂控制系统，得到系统运动误差的最大值为18 mm[8]。王绪全对六自由度机械手抓取定位精度进行研究，发现机械手虽然达到预期的控制目标，但抓取精度有待提高[9]。因此，提高机械手的控制精度是实际应用中需要重点解决的问题。机械手的控制精度不仅与舵机本身的精度密切相关，而且与机械结构、控制电路和程序有关。

针对教学用自动化生产线实训系统的人工上料环节，拟配备机械手进行物料的抓取和放置。本文设计了以单片机IAP15W4K58S4为核心的机械臂控制系统，主要包括电源接口电路、程序下载电路和无线信号收发电路的设计，以及部分程序模块的编写，实现PWM舵机驱动六自由度机械臂抓取料仓内的物料并放到上料转盘上。单片机输出的PWM信号进入舵机内部后，与舵机内部的基准信号相比较，产生一个有大小和方向的压差信号，该信号驱动减速齿轮和舵盘转动，位置反馈电位器反馈相应的电压给控制系统，使得电机旋转一定的角度[10]。单片机编程使PWM信号的脉冲宽度实现μs级的变化，从而提高舵机的转角精度，改善控制系统的控制精度。通过Proteus仿真PWM舵机的输出结果，辅助分析程序的正确性以及预测舵机运行结果。因为各个关节的运动有交互影响，所以通过正交试验测试机械臂末端中心的位置，与理论推测位置相比较，得到定位误差。

1 机械臂结构

常见机械臂的运动坐标形式有4种：直角坐标型、圆柱坐标型、极坐标型和多关节型，其中圆柱坐标型和多关节型在工业机械臂中最常见[11-12]。多关节型机械臂多用于小型空间、轻载和灵活性较高的场合。本设计中，物料分层放置于长方形料仓中，机械手固定在工作台上，其中心位于料仓与上料台之间，被抓取物料的质量不超过0.5 kg，所以选择多关节型机械臂。为了实现长方形料仓中上下、前后、左右物料的抓取，机械臂需要6个自由度，实现水平面内的转动，垂直俯仰和屈曲，手抓旋转和开合[12]，对应机械臂的底座±90°旋转，大臂、小臂和手腕±90°屈曲，手抓±90°旋转及±90°开合。因此，该关节型机械臂的6个转动关节如图1所示，其中，关节1实现机械臂底座在水平面内的旋转，关节(2、3和4)分别实现大臂、小臂和手腕的屈曲,关节(5和6)分别实现手抓的旋转和开合。由于舵机具有时间常数小、起步转矩大、线性度好、反应迅速等特点[13]，本设计采用6个齿轮舵机驱动6个关节动作，通过精密电位器检测舵机的转角，实现料仓内指定位置物料的抓取和放置。

图1 六自由度机械臂示意图

2 控制系统总体设计

系统的最终控制对象是PWM舵机，其控制信号是占空比可调的PWM波。鉴于单片机编程较灵活，可以产生精度较高和较稳定的PWM信号，所以本设计采用单片机作为主控芯片来控制PWM舵机。根据PWM波的产生和串行通信的方式，对单片机进行选型。STC的IAP15W系列单片机是一种宽电压(2.5～5.5 V)、高速、高可靠性、低功耗的中央处理单元，有8路PWM，4组超高速异步串行通信口和1组高速同步串行通信端口SPI[14]，可以满足本设计的要求。因此，采用STC的IAP15W4K58S4单片机作为主控芯片，实现多个舵机的同时控制。

机械臂控制电路主要包括单片机IAP15W4K58S4最小系统、电源电路、程序下载电路、按键电路、无线信号收发电路等，如图2所示。舵机和单片机有各自的电源电路，以保证输出电压稳定在各自的电源范围内，不受输入电压波动的影响。程序下载电路实现计算机与单片机之间的串行通信，用于计算机给单片机下载程序和发送控制指令。程序下载到单片机后，通过键盘电路的按键控制舵机的转角。通过无线信号收发电路，实现手柄对舵机的遥控。

图2 机械臂控制系统组成框图

3 控制系统硬件电路设计

3.1 单片机最小系统

IAP15W4K58S4单片机虽然有片内晶振和复位电路，但片内晶振的精度不高，为了保证系统的工作时序和信号传输的准确性，本设计增加片外晶振电路，如图3所示。为了方便人为操作单片机系统从预先设定的起始状态开始工作，增加按键复位电路，与上电复位电路一起作为外部复位电路，如图4所示。

图3 晶振电路

图4 复位电路

3.2 电源电路设计

机械臂控制系统中，舵机的工作电压为6 V，单片机的工作电压为5 V，无线通信芯片的工作电压为3.3 V。为了使各个器件都能在对应的工作电压下正常工作，设计了具有不同输出电压的电源电路。系统电源采用2节18650锂电池串联，得到7.4 V供电电压。舵机电源电路采用XL4016开关降压型DC-DC转换芯片，将7.4 V供电电压降为6 V并稳压(图5)；5 V电源电路采用AMS1117-5.0正向压降稳压器，将7.4 V电压降为5 V并稳压(图6)；3.3 V电源电路采用AMS1117-3.3正向稳压器，将5 V电压再降为3.3 V并稳压(3.3 V与5 V电源电路的原理相似)。

图5 舵机6 V电源电路

图6 单片机5 V电源电路

3.3 程序下载电路设计

IAP15W4K58S4单片机有4个全双工异步串行口，计算机可以通过单片机的串行口实现程序的下载。计算机通过USB口连接到程序下载电路，如图7所示，CH340G将计算机发来的USB电平信号转换为TTL电平信号后，通过串口连接到单片机的RXD1和TXD1串行口，实现计算机给单片机下载程序的功能。

图7 程序下载电路

3.4 无线信号收发电路设计

目前有多种无线通讯模块，如WiFi、蓝牙、ZigBee等。考虑到手柄遥控距离不超过5 m，本设计选用2.4～2.48 GHz频段的低功耗无线射频收发芯片NRF24L01，其工作电压为1.9～3.6 V(通过3.3 V稳压电路供电)[14-15]，编程简单，与无线信号收发电路的主控单片机STC12C5A60S2实现无线通信，如图8所示，将NRF24L01的引脚3～8连接到STC12C5A60S2对应的接口上，STC12C5A60S2将从NRF24L01接收到的信号进行处理后，再通过串行口TXD和RXD连接到IAP15W4K58S4的RXD2和TXD2，进行全双工通信，实现对手柄遥控信号的收发。

图8 无线信号收发电路

4 程序设计与软件仿真

4.1 程序设计

采用模块化程序设计思路，控制程序由主程序、A/D转换子程序、串口通信子程序、PWM舵机控制子程序、定时器中断子程序及按键子程序等模块组成。其中，A/D转换子程序对供电电源电压进行实时监测，如有异常通过蜂鸣器报警；串口通信子程序实现IAP15W4K58S4和STC12C5A60S2之间的通信，从而接收遥控手柄的控制信号；PWM舵机控制子程序通过单片机的定时器控制PWM波高电平的输出时间，从而改变PWM信号的占空比，达到控制舵机转角的目的。

主程序中，先进行主函数全局变量和局部变量的声明、子程序声明、引脚设置(准双向口工作模式)等。再进行舵机PWM初始化、A/D转换初始化、串口初始化、定时器初始化、蜂鸣器初始化等。随后开启中断，程序进入循环执行状态，直到人为按键或断电停止。主程序流程如图9所示。

图9 主程序流程

通过IAP15W4K58S4自带的A/D转换子程序获得电源电压值，并判断电源电压是否低于系统设定值7.2 V，若低于设定值，则启动蜂鸣器报警，实现对锂电池输出电压的监测。

串口通信子程序中，串口初始化后执行串口中断程序,每次接收4个字节存入缓冲区。当串口接收完标志fUartRxComplete为1时，表示数据接收完毕，然后执行Task_MCUHandle()函数，实现手柄对舵机的控制。串口通信子程序流程如图10所示。

图10 串口通信子程序流程

舵机需要的PWM控制信号是周期为20 ms的方波，通过单片机的定时器输出高电平时间在0.5～2.5 ms内变化的PWM信号，驱动舵机在-90°～90°之间转动。PWM舵机控制子程序中，首先对舵机控制变量和定时器0初始化，进入定时器0中断程序后，判断循环次数i的值，若i<12，则根据舵机初始位置数组值，对定时器0进行重装；若i≥12，则将ServoPwmDutyHaveChange置1，说明6个舵机均已到达目标位置。PWM舵机控制子程序的流程如图11所示。

图11 PWM舵机控制程序流程

4.2 软件仿真

单片机可以给6路舵机同时发送PWM信号，实现6路舵机的同时运行，模拟实际中多个关节的协调运动。为了更直观地观察PWM程序控制舵机旋转的角度，对PWM子程序进行软件仿真。首先在Keil中编辑程序，经编译连接后生成hex文件，再将该文件下载到单片机。仿真时，舵机选用Proteus中的MOTOR-PWMSERVO，运行后得到Proteus仿真结果(图12)，每个舵机下方显示的数字仿真结果对应各自旋转的角度。

图12 Proteus电路仿真结果

Proteus自带的虚拟示波器可以显示仿真的PWM波形，据此初步判断PWM控制程序和改变占空比时PWM波形的变化是否正确。PWM程序的示波器仿真结果如图13所示，示波器输出的PWM波形呈周期性矩形波，符合PWM舵机的控制要求，初步确定舵机能正常运行。

图13 PWM波形仿真结果

5 实验测试

将调试好的电路与机械臂的各个舵机连接，将前5个舵机的旋转角度分别作为1个控制因素(第6个舵机控制的是手抓的开合)，将每个舵机的旋转角度(-90°～90°)等距设置为4个水平(-90°，-30°，30°,90°)，开展5因素4水平的16组正交试验，将所有舵机的初始位置调整到其可达范围的中间(0°)，通过程序控制5个舵机运动对应的角度，测量机械臂末端中心(不含手抓)在空间3个方向的位置，并与理论计算的空间位置相比较，所得偏差即为定位误差，得到3个方向的定位误差均在-2～2 mm范围内。

6 结束语

本文主要对自动化生产线实训系统上料环节的六自由度上料机械手的控制系统进行研究，所得主要结论如下：

(1)从机械臂的结构形式和驱动形式出发，确定采用关节型机械臂和齿轮传动PWM舵机，通过改变PWM信号的占空比来控制各个关节舵机旋转的角度。

(2)机械臂控制系统选择IAP15WK458S4作为主控芯片，硬件电路采用模块化设计，包括电源电路、单片机晶振电路、复位电路、程序下载电路、无线信号收发电路等，实现了不同电路模块电源电压的供给和监测、计算机程序的下载、手柄遥控信号的传输等功能。

(3)采用模块化程序设计思路，对机械臂控制程序进行了设计。主要包括主程序、A/D转换子程序、串口通信子程序和PWM舵机控制子程序。利用Proteus对PWM舵机控制子程序进行了仿真，发现6路舵机可以按照指令要求同时协调工作。

(4)通过正交试验测试机械臂末端中心在前5个关节舵机驱动下的定位误差，多次实验运行结果表明当负载在0.5 kg范围内，机械臂末端中心3个方向的定位误差都不超过2 mm。