基于STM32的小型机械手的设计与控制方法

刘士川，邓甲昊，占银玉，叶 勇

(1.北京理工大学机电动态控制重点实验室，北京 100081；2.北京理工大学机电学院，北京 100081)

基于STM32的小型机械手的设计与控制方法

刘士川1，2，邓甲昊1，2，占银玉1，2，叶 勇1，2

(1.北京理工大学机电动态控制重点实验室，北京 100081；2.北京理工大学机电学院，北京 100081)

针对机器手的每个关节需要进行位置和速度的控制，而伺服电机、步进电机等控制存在成本高，控制精度不够等问题，提出了基于STM32的小型机械手的设计与控制方法。相对于电机控制来说，基于脉宽调制技术控制的舵机机械手臂有其控制简单、灵活和动态响应好、价格便宜等优点。设计了机械手臂的整体结构，分析了码垛机械手的脉宽调制驱动方法，并在结构设计的基础上进行了运动学分析，得出了机械手的末端位姿。对机械手抓取目标物的控制系统进行了设计、编程及仿真实验，采用实体建模软件Solidworks建立小型码垛机械手臂虚拟样机模型。基于该模型，设计了抗环境噪声与电路自身噪声问题的电路，进行多次搬运实验。实验证明，该方法可以实现机械手对物体的准确抓取、运送等工作，有效地避免了电机定位不准、价格昂贵的情形，使得控制更为简单灵活。

脉宽调制控制；机械手；运动学分析；结构建模

0 引言

码垛技术是现代工业生产以及物流运输上一项必不可少的技术，该技术的成熟程度关系到工业加工的效率以及工业运输的时间问题，近几年在工业生产中扮演着越来越重要的角色[1]。机械手(也称末端执行器)是码垛机器人的重要组成部分之一，其工作性能、可靠性、结构等参数关系到码垛机器人的整体工作性能的好坏。机器手的驱动可以有很多选择，比如直流电机、伺服电机、舵机、步进电机。由于机器手的每个关节需要进行位置和速度控制，直流电机要实现上述控制必须增加位置和速度传感器，这就增加了安装困难和机器人成本。伺服电机虽然可以满足机械手的控制，但是伺服电机本身体积大、价格昂贵，很难应用于机械手上。步进电机体积小、价格便宜，但是比较重、控制精度不够。本文针对上述问题，采用舵机控制，提出了基于STM32的小型机械手的设计与控制方法。

1 机械手系统总体设计

1.1 小型码垛机器手总体方案

本文主要是针对码垛机器手设计一个机械手的结构和控制系统，采用上位机和下位机结合的方式完成和实现搬运任务。一方面通过上位机遥控实现机械手上下抓取物体、姿态调整、转动搬运物体等基本功能；另一方面机械手可以通过自身的红外测距传感器和姿态传感器实现自身动作的调整和姿态的变换，并将姿态信息记入FLASH存储中，每个完成的动作通过底座外接的LCD1602显示屏展示出具体情况。上位机和下位机通过无线通讯的方式来传递机器人的控制命令和传感信息，下位机则是通过ARM完成对各个模块的控制，负责机器手的动作控制和姿态存储，并将传感信息传递给上位机，以达到精确控制机械手动作的完成。机械手的系统整体框图如图1所示。

图1 机械手系统整体框图Fig.1 Manipulator system block diagram

码垛机械手的整体机械结构如图2所示。该小型码垛机械手臂机械部分由4个部分组成[2-3]，包括前段的抓手部分、小臂、大臂、底座，总共三个自由度，完成这三个自由度一共需要4个SG-90舵机，分别安装在小臂与抓取部分的关节处、小臂与大臂的关节处、大臂与底座的关节处、底座最下端的扭转处。它在结构上区别于一般的关节型机械手，它的大臂和小臂外侧各有两个平行四边形辅助连杆结构，这种设计结构使得这个机械手有两个特点：一是机械手有三个自由度，两个平移和一个转动，从而使得末端抓手能够始终稳定地保持抓取过程中的水平姿态；二是两侧的平行四边形辅助结构通过舵机衔接，使得手臂两侧等速连接，获得平稳的运动效果。

图2 小型码垛机械手的整体结构Fig.2 Small palletizing robotic arm structure

1.2 小型码垛机械手的驱动方法

本文采用的核心控制芯片为STM32芯片，是ARM Cortex-M内核的32位微控制器，有休眠、停止、待机3种低功耗模式，采用ECOPACK封装，最高工作频率72 MHz，1.25 DMIPS/MHz。采用固定频率、电压模式结构对信号进行控制，可应用于舵机控制、Flash动画、温度调节等领域。

码垛机械手的运动研究主要是对电机的控制，小型码垛机械手实验平台包括四个舵机，其型号为SG-90。码垛机械手的驱动方法采用脉冲宽度调制，脉宽调制技术可利用微处理器的数字输出实现对模拟电路有效地选择与控制，产生周期性的脉冲调制信号，然后通过调节相应的占空比来对舵机的角度进行调整，从而实现对机械手的运动控制。具体的设计过程为设计定时器计数从0到999共1 000数为一个定时周期，设置预分频为160，采用向上计数，配置PWM模式，设置跳变值，当计数器计数到这个跳变值以后进行跳变，低于这个值时为高电平输出，设置4个使能通道，分别能够输出4个脉宽调制，采用比较模式寄存器，在程序末端使能重新装载，用修改定时器中断初值的方法形成了脉冲信号，调整时间段的宽度便可使伺服机灵活运动，从而能根据输出的脉宽调制进行舵机角度的控制。

2 机械手末端控制运动学分析

2.1 机械手正向运动学分析

机器人运动学是指在不涉及产生运动的力或力矩的情况下，研究机器人末端执行器相对于固定参考坐标系的运动几何学关系[4-5]。机器人运动学研究的实质就是考虑机器人位置、速度和加速度信息，有时甚至考虑机器人位置与时间的更高阶导数。本小型码垛机械手根据正向运动学即已知各个关节角的大小，求解机器人末端的位姿矩阵。机器人的正运动学可以通过各杆件的变化矩阵相乘得到，在各关节变量确定后，其末端的位置也随之确定。对于小型码垛机械手，可基于D-H参数建立运动学模型。D-H参数是为描述相邻杆件间平移、偏移、旋转和扭转的关系，通过在每个关节处的杆件坐标系建立4×4齐次变换矩阵，表示它与前一杆件坐标系的位姿关系。采用修正的D-H参数模型建立坐标系，连杆的4个参数确定规则如下：

1)杆长ai：Zi-1沿Xi轴平移的距离；

2)关节扭转角αi：Zi-1绕Xi-1轴旋转的角度；

3)两连杆距离di：Xi-1沿Zi-1轴平移距离；

4)旋转角度θi：Xi-1绕Zi-1轴旋转的角度。

其D-H参数如表1所示。

表1 D-H参数表

工具坐标系与第4坐标系关系为：α=0，a=y0，d=0，θ=0。

当码垛机械手的手抓指尖位于要搬运货物点，且手抓中心线位于搬运货物的法向时，搬运的效率最高。基于此考虑，假设工作位置为(x0，y0，z0)，工件曲面在此处的法向为{nx，ny，nz}。结合以上分析可知坐标系的原点位置为p=(x0，y0，z0)，y轴的方向向量为n2={-nx，-ny，-nz}。构建结构图如图3所示。

图3 小型码垛机械手结构简图Fig.3 Small palletizing robotic arm schematic

根据表1和修正D-H参数法的相关准则，得到各连杆之间的变换矩阵如下所示：

变换矩阵是3个关节变量的函数,称为机器人运动学方程，它将机器人的位姿从关节空间描述变换为直角坐标空间描述。有时为了简化运算，把常量T通过矩阵逆运算移至等式左边，从而直接计算T，这样计算的结果意义是第4坐标系相对于极坐标系的位姿。变换矩阵T的意义是末端坐标系相对于极坐标系的位姿。

上式中各量所代表的量如下：nx=c1c2s3-c1s2，nx=c2c3s1-s1s2s3，nz=s2c3+c2s3，ox=-c1c2s3-c1c2c3，oy=-c2s1s3-s1s2s3，oz=-s2s3+c2c3，ax=s1，ay=-c1，az=0，px=c1c2L2L3-s1L1，py=c2c3s1L3+c2s1L2+s1c1，pz=s2c3L3+c2s3L3+s2L2；其中，si代表sinθ，ci代表cosθ。

2.2 机械手逆向运动学分析

姿态矩阵的第1，2，3列分别表示工具坐标系的xt，yt，zt轴与基坐标系的三个轴的夹角余弦，(px，py，pz)为工具坐标系原点在基坐标中的位姿[4]。在货物点和该点法向量已知的条件下，可以求出位姿矩阵，继续求解时，是一个循环体，循环变量为θ，每一步增加一定角度，求出机器人的位姿矩阵，然后按照3自由度机器人求解，求出的关节角度即为码垛机械手的关节角度。

1)求关节角θ1

由上式可知：ax=s1，

反解出：θ1=sin-1ax。

2)求关节角θ2

由上式可知：px=c1c2L2L3-s1L1，ay=-c1，ax=s1，

3)求关节角θ3

由上式可知：py=c2c3s1L3+c2s1L2+s1c1，

4)解的整理

由计算得到了θ1、θ2、θ3三个解。在工程上，根据关节运动范围和工作空间的约束，根据位置或能量最小变化原则，选取与位姿最为接近的运动节点，根据实际需要改进上述理论解去满足末端位姿条件。

3 模型建立与实验验证

3.1 机械手虚拟样机的搭建与轨迹仿真

搭建Solidworks软件仿真平台，采用Parasolid核心实体造型技术，建立机械手的虚拟样机模型如图4所示。将模型导入到ADAMS中，定义机械系统模型，创建刚体，得到运动轨迹仿真，如图5所示。

图4 机械手虚拟样机模型Fig.4 Virtual prototyping

图5 机械手运动轨迹Fig.5 Motion trail

3.2 小型机械手控制电路设计实现

电子产品更新周期快、个性化差异大的形势，为进一步加快研制步伐，同时最大化地降低研制成本，采用电路模块化的设计理念和方法[6-8]。本小型搬运机械手控制系统硬件组成为传输模块、芯片控制模块、电源转换模块、外界端口模块、扩展控制模块。

电源通过TP4056芯片对为控制手柄内芯片进行充电，充电时7引脚输出低电平，红灯D2亮起；6引脚输出高阻，绿灯D1灭；充满时7引脚输出高阻，红灯D2灭，6引脚输出低电平，绿灯D1亮起。充电电路如图6所示。

图6 机器手充电电路Fig.6 Machine hand charging circuit

外接电源采用AMS1117芯片进行电压之间的转换，它的稳压调整管是由一个PNP驱动的NPN管组成的，将供电电压转换为3.3 V为主控STM32芯片进行供电。nRF24L01芯片工作ISM频段2.4～2.5 GHz，通过SPI接口设置频道选择和协议，实现远程遥控。启动舵机后，先进行电路复位初始化，将所有手臂竖直向上放置，通过MPU6050姿态控制芯片进行姿态调整，并将数据记入在FLASH中存储，在对功能进行分解后，结合机器手臂的运动要求，设计出如图7的控制架构框图。

图7 机器手电路控制架构框图Fig.7 Machine control architecture block diagram of hand robotic arm

3.3 机械手脉宽仿真验证

利用STM32分别输出5%，50%，37.5%，25%占空比的四路PWM波程序图，采用计数的方式输出不同占空比的波形，通过电脑仿真产生如下展示的波形对舵机进行相应角度的控制，如图8所示。

本小型实验平台所采用的4路信号控制，用来实现对4路舵机的同时控制，并产生四路信号输出，通过多路脉冲信号来实现抓手与机械臂之间的协调运动。首先机械臂处于静止状态，当抓取物体成功后，机械臂开始运动，此时的抓手处于静止咬合状态。当机械臂运动到规定位置以后，机械臂下放机械手，使得机械手能够卸下物体，然后机械臂带动机械手回归初始位置，再重复上述过程。

图8 脉宽调制仿真Fig.8 Pulse width modulation simulation

4 结论

本文提出了基于STM32的小型机械手的设计与控制方法，设计了一款采用脉宽调制技术的舵机机械手臂。该方法利用STM32控制简单、精确度高的特点，使得开发工作量比起传统的嵌入式系统大大减少。经过试验，实现3自由度机械手在人工控制和自动控制两种模式下对重物的抓取，并能通过姿态传感器MPU6050得到姿态的测量和修正，通过FLASH实现存储。

本文重点研究了多舵机条件下以脉宽调制技术为基础的联合控制方法，并结合多个传感器设计出控制系统，该系统有很好的通用性，为后续研究提供了借鉴与参考。

[1]胡洪国,高建华,杨汝清.码垛技术综述[J].组合机床与自动化加工技术，2000(6)：7-9.

[2] 付铁,李金泉,陈恳，等.一种新型高速码垛机械手的设计与实现[J].北京理工大学学报，2007(1):17-20.

[3]曾孔庚,王宏庆,丁原彦.高速机器人搬运码垛系统构成及技术特点[J].机器人技术及应用,2001(4)：39 -42.

[4] 姚猛,韩宝玲,罗庆生，等.工业码垛机器人机构设计与运动学分析[J].组合机床与自动化加工技术,2011,(5):31-33,37.

[5] 李建伟.码垛机器人机构设计与运动学研究[J].机械设计与制造，2009(6)：181-183.

[6] 肖永强.三自由度机器人及其控制系统研制[D].哈尔滨: 哈尔滨理工大学，2008.

[7] 马纲,王之栎,韩松元.一种新型搬运码垛机械手的设计[J].机械设计与制造,2000(4)：26 -27.

[8] 刘永.弧焊机器人工作站智能化技术研究[D].南京：南京理工大学, 2008.

Design and Implementation of Manipulator Based on PWM Technology

LIU Shichuan1,2, DENG Jiahao1,2, ZHAN Yinyu1,2, YE Yong1,2

(1. Science and Technology on Electromechanical Dynamic Control Laboratory, Beijing 100081, China; 2. School of Mechatronical Engineering, Beijing Institute of Technology, Beijing 100081, China)

In this paper,we presented the STM32 design of small robots and control method for machine hand of each joint, which need low cost and high precision location and speed of control. For the electro-mechanical control, this method was easy and flexible, it had the fast dynamic response and the expense was cheap. First of all , we introduced the architecture of the palletizing manipulator, analyzed the PWM technology. Secondly, we analyze the kinematics characteristic based on the structure to get the position of the manipulator end effector. We carried on the design, programming and the simulation experiments to the robot-control system, adopted 3D entities modeling software Solidworks which had established the virtual prototype model of the palletizing manipulator.

pulse width modulation；manipulator；kinematic analysis；structural modeling

2016-12-05

刘士川(1993— ), 男，山东德州人，硕士研究生，研究方向：电容引信探测、机器人、信号处理。E-mail:liushichuan@bit.edu.cn。

TP242.6

A

1008-1194(2017)02-0019-05