基于单片机的六自由度机械手臂控制系统设计

陈心怡 张春雨 朱丽华

（1.池州职业技术学院，安徽 池州 247000;2.安徽科技学院，安徽 滁州 239000）

一、引言

机械臂也称工业机器人，是以运动作为控制对象的智能控制装备，主要由手臂、舵机、抓手等组成[1]。机器人手臂的张开、夹紧等系列动作由电动机驱动，并准确地反馈到可编程逻辑控制器。在工业生产中控制机械臂完成我们所需要的夹取和分拣动作，本项目便是围绕六自由度机械臂控制系统展开的。通常一个52系列的单片机包含有FLASH ROM、RAM、3个16位的定时器/计数器和1个UART等，它具有性能可靠、性价比高等特点。在工业生产中，往往需要机械手臂完成一些相对复杂的控制运动，机械手臂系统中一个单片机远远不够，仍需要TK-A66自由度机械手臂、1501数字舵机等控制机械臂的运动。

本文就是用单片机作为核心控制器，LD1501-MG数字舵机作为TK-A66自由度机械手臂的运动控制单元，结合制成的6自由度机械臂的控制系统。系统采用单片机芯片STC89C52作为主控制器，A/D转换采用以双积分方式运行的ICL7135，利用定时计数器的计数功能，测量外部电压，省去很多处部电路[2]。单片机通过产生PWM信号控制机械臂的舵机，从而控制6自由度的机械手臂。

二、算法设计

（一）建立运动学模型

如图1所示，具有三维开环链结构的机械手分别由基座、增幅臂和旋转手臂组成，可通过肩部旋转和俯仰、肘部和腕部俯仰、腕部偏转和旋转6个旋转关节实现6个自由度的运动，进而保证机械手在工作空间中的所有位置状态[3]。

图1机械臂模型

为准确反映关节坐标与机械端位置之间的关系，本文根据D-H原则和笛卡尔坐标系建立机械臂关节的坐标系及机械臂的正向运动学方程，但逆运动学模式在实际应用过程中更为普遍[4]。机械臂控制系统的运行可以通过调整关机位置来实现，这就需要通过逆运动学模型，计算出完成理想轨迹时每个关节所需达到的角度值。在确定了机械臂的构型后应以D-H法则确定运动学参数，如表1所示。

表1 D-H参数表

根据图1中的构型建立参考坐标系和关节坐标系，再用齐次方程描述机械臂各连杆的空间几何关系，用4×4齐次矩阵来表示相邻连杆之间的几何关系，再算出机械臂相对于参考坐标系的后端地址。鉴于该坐标系中存在偏移量α1和α2，故χ1、χ2和χ3的空间关系并不重合，如图2所示。

图2 1,2,3坐标空间关系

（二）运动控制算法

运动轨迹和速度是机械臂两大控制变量，轨迹控制算法是机械臂控制系统的最关键部分。相对于二维轨迹控制算法，六自由度轨迹控制算不是二维插补算法的简单组合，综合曲线特征，采用分段曲线逼近拟合轨迹曲线。为了提高拟合度，须精确计算出各段曲线交点值，以保证运动状态参数的连续性[5]。考虑到既要使机械臂能沿着既定的轨迹运动，又要确保运动过程中的实时可控性，本文采用Trial Mode运动模式的控制算法，即通过阶梯形分割其速度曲线，在每个采样周期内分段进行提取设计，确保对插补算法指定轨迹进行约束。

（三）调试运动规划算法

合理设计机械臂算法调试软件，本文软件采用C语言编写算法，借助WinCE系统平台构建其运动规划算法的实验平台，采用MFC设计合理的程序框架，来验证算法功能以确保算法函数的可行性与控制的精准度，确保对不同运动模式的仿真验证。

三、硬件设计

将硬件设计过程分为单片机模块和舵机控制模块两个部分，系统原理图如图3所示。在机械臂硬件设计过程时，其构成部件的选择应匹配于机械臂的基本技术参数，之后根据对关机转矩等计算推演，合理选择满足功率要求的电机。综合考虑，本硬件设计中核心的器件选用STC89C52型号的微处理器，内置8KB可在线编程闪存，具有A/D转换功能，并能产生PWM信号。另一方面，产生的PWM信号控制机械臂舵机的制动[6]。

图3系统原理图

（一）单片机模块

单片机模块所使用的STC89C52单片机，包括单片机芯片、复位电路、时钟电路和电源电路。该芯片采用CMOS工艺，具有密度高、存储信息不易丢失的特点，与C51指令集兼容。Internal RAM允许重复在线编程，还可通过SPI串行端口或同一非易失性存储器进行重新编辑。通过将8位CPU与可线上下载的Flash进行集成，可有效地解决复杂的控制问题。内部系统包括8K字节可在线编程ISP闪存，4.0-5.5V电源，完全静态工作0Hz～24MHz，256字节内部RAM，32根可编程I/O线，8个中断源和双数据指针，2个16位定时器/计数器。

（二）舵机控制模块

本系统采用数字舵机LD1501-MG分2组进行实验，第一组中舵机与单片机共用同一电源，第二组中使用不同电源供电。数据表明，第一组单片机运行不稳定，第二组中两者运行较好。所以本系统采用舵机与单片机公用不同电源方案，且舵机所需的电源功率要大于单片机的电源功率，如图4所示[7]。

2）其他土地利用类型都有不同程度的减少，其中水域和耕地减少的最为明显，1985－2000年和2000－2016年水域的减少速度分别为0.08%和0.19%，耕地的减少速度分别为0.12%和0.14%；

图4舵机驱动电路图

四、软件设计

（一）总体设计

本设计利用上位机操作系统及监控程序实现信息通讯、轨迹规划及运动算法控制等功能[8]。根据操作步骤，上位机提取INI文件和数据库中的参数，并在可视化界面中轻易完成机械臂轨迹规划和关节角位置的设定。信息通信过程首先要初始化通信端口，通信总线与各关节进行通信连接，发出连接信号并读取数据，进而发出指令并驱动电动机运动。为兼顾系统软件功能可以同步完成，本控制系统采由C51语言编程来实现，程序结构采用模块式，由主程序、初末置换函数、速度控制函数、机械臂控制函数、定时器T0子程序、定时器T1子程序6部分组成。结构清晰，便于分析调试。

（二）主程序设计

主程序是将整个程序分成若干模块，在程序中利用函数调用的方式，连接各个子程序，传递之间的参数。系统的主程序按照以下流程执行。

程序开始，对全局变量进行声明，对相关数据及参数初始化，3个定时器数据初始化，将输出引脚设置为强推挽输出，为舵机提供足够的电流。中断设置，定时器初始化并开启中断，设置定时器TCON和TMOD参数。

中断请求，接到信号请求则调用pwm[]数组更新函数舵机调速子程序vpwm()。流程图如图5所示。

（三）舵机控制程序设计

图5程序流程图

为保证机械臂平稳运行，需要对舵机进行精确控制，以实现速度调节，实际上在程序中对舵机控制信号PWM占空比进行调节即可。PWM是脉冲宽度调制信号，具有易控制、迅速和动态响应好等特点，被广泛应用在人工智能机器人及航模设计的舵机及直流电机的控制中，其PWM设置格式应注意以下几点：

（1）上升沿保证有0.5ms，介于0.5ms至2.5ms之间；

(2)要求用程序实现连续的PWM信号，对于硬件体现出来的机械臂动作更加连续、流畅。

图6多路舵机PWM速度控制程序流程图

五、系统仿真与实现

为使控制系统的开发更加的高效便捷，在进行硬件电路的焊接前，先进行仿真。本控制系统设计，用Protues仿真软件搭建仿真系统的电路图，由于舵机在Protues元器件库中没有加入，本仿真可用Protues自带的虚拟示波器Oscilloscope代替。通过观察Oscilloscope显示的PWM控制信号的占空比，判断控制系统的控制性能。可根据仿真结果，通过调整单片机程序，改变占空比，从而得到电机平均速度，达到调速的目的。

硬件调试阶段首先测试舵机，将舵机测试程序烧录到单片机系统中，测试并记录舵机偏转的角度并与预期角度进行比较，误差角度控制在±1度以内，对不能达到要求的舵机不予选用。选择好舵机以后根据机械臂结构图组装好机械臂，通过软件编程程序控制机械臂实现6自由度运动。最后，组装硬件并反复调试。在调试的过程中，根据实际情况，机械臂的重量，平稳程度等，可通过编程来控制发送PWM信号的时间，以适应不同距离的测量需要，以达到准确抓取目标的任务，确保实现6自由度机械臂的运动控制功能。

六、结语

机械臂作为应用广泛的运动执行机构，其运动稳定性在现代工业生产中意义重大。本方案在反复比较国内外同类产品的基础上，深入分析其适用行业特点和功能需求，确定总体设计方案使用STC89C52作为主控制器、LD1501-MG数字舵机作为控制单元，主要由机械臂电路与单片机控制电路两大部分构成。

机械臂控制系统中建立运动学模型，运用TM算法，对机械臂手臂的运动速度和轨迹进行控制设计，实现机械臂手臂运动轨迹的控制功能。在本系统控制程序中，利用单片机产生的PWM信号对舵机进行控制，给出了微分插补法生成多路舵机PWM速度控制信号的程序实现方法，通过微分插补法进行舵机速度的控制，实现机械臂状态的控制和调整，使机械臂的运行更加平稳。此系统的仿真实现受限于无障碍的空间环境，其控制和轨迹算法应结合不同的复杂环境进行跟进调整，使其功能和应用价值更为凸显。