刘琪

（江苏安科瑞电器制造有限公司，江苏无锡, 214400）

0 引言

在日常生活中，机械手通常用于夹持物体，很多机械手采用开环控制，这样就导致当机械手夹持力过大时，被夹持物破裂，影响其使用，当夹持力太小时会导致机械手夹持松弛，不能稳定夹持以进行自动化作业。以试管为例，试管属于材质比较脆弱的物品，常用于实验，需要对夹持力精准地控制，不能在夹持过程产生大的超调，不然可能会夹碎试管，所以能很好的测试系统效果。夹持时由于试管会产生形变而导致夹持力变化，所以应实时检测试管所受压力值，不断调整控制输出以保证稳定夹持。而且无外置传感器的方式，在实际应用中会更实用，更好推广，不需要在机械手上安装复杂的传感器，简化了安装。

1 系统整体方案

夹持力系统是靠控制无刷直流电机的转速来控制机械手的夹持力，控制的夹持力与电机的转速呈一定比例关系，所以本系统通过构建一个速度闭环来间接控制机械手稳定夹持试管。

整个系统主要由机械手、STM32开发板、电机调速器、无刷直流电机几部分组成。首先给定一个目标速度值，机械手开始慢慢夹持试管，电机调速器将检测到的电机位置、速度和力矩信息通过CAN总线反馈到输入端，这时给定速度与反馈回来的速度的差值输入控制器，控制器按PID控制算法处理输入信号并产生输出，通过CAN总线发送控制指令给电机调速器，从而控制电机驱动机械手夹持试管，产生新的速度值，形成闭环控制。系统框图如图1所示。

图1 夹持力闭环控制系统框图

2 系统数学模型及仿真

本设计采用的PID控制器，它的传递函数表示为：

根据直流无刷电机的数学模型，其传递函数为：

则本夹持力控制系统对应的数学模型如图2所示。

图2 夹持力控制系统数学模型

PID控制主要包含3个控制参数：P（比例）、I（积分）、D（微分），根据搭建的系统，对输入信号处理，实现无偏差输出。本设计中选用增量式PID控制策略。由下面公式可知，增量式算法只与最近3次偏差值有关，工作量小，不仅能够保证很好的实时性和快速性，而且不会产生很大的累积偏差，对系统影响较小，出现意外还可以输出限幅和积分限幅等方式限制本次输出。

增量式PID的表达式：

其中：k为采样的序号，err(k)为第k次的误差，u(k)为输出量，Δu(k)为第k次输出量与第k-1次输出量的差，在这里，为了简便计算，方便说明，令Ki=Kp*T/Ti、Kd=Kp*Td/T，在后文统一用Ki和Kd来代表积分、微分环节的性能参数。

本课题采用试凑法整定PID参数，根据夹持力系统的数学模型，把仿真需要的特征参数带入，在整定过程中，根据系统过渡过程调整PID参数，边观察过程曲线特点，边根据PID的3个参数各自功能修改对应参数值，直到响应曲线满足速度、静差等设计要求。

建立仿真模型如图3所示，给定夹持力为3N，经过PID参数的整定过后，整个系统的响应曲线如图4所示。整个过程在开始经历了少量的超调后，能够较快的稳定在给定值3N附近。在整定的过程中，加快响应速度往往以提高超调量为代价，综合考量后，采用第一组PID整定参数，在保证超调量不高的情况下，保持一定的响应速度，提高了系统整体的性能。

图3 夹持力控制系统仿真建模

图4 夹持力控制系统响应曲线

3 夹持力系统硬件组成

本设计采用STM32F103C8T6芯片作为主控芯片，其最小系统包括电源、晶振和复位三部分组成，电源为开发板上器件供电，晶振为系统提供准确的时钟信号。结合控制需求，控制器需要5V和3 3V两组电压，通过带USB接口的电源接入电路为控制器和其它硬件电路接入5V电源，并采用AMS1117-3 3芯片，将5V电源转换为3 3V供给需要的硬件电路，保证整个硬件电路系统稳定运行，不会出现欠压或过流。同时，设计的电源指示灯电路可以清晰地显示3 3V供电是否正常。

控制器按PID控制算法处理输入信号后，需要通过CAN总线把控制指令发送给电机调速器。这里选用CAN收发器为TJA1050，它是一款独立CAN收发器，在应用方面相当普遍和成熟，可以满足大部分数据通信需求。报文的仲裁场、控制场、数据场的数据由软件编程配置TJA1050，报文的帧起始、CRC场、应答场、帧结束由TJA1050自动配置完成。为了保障数据正常传输，确保可以接收到无刷电机调速器的反馈，设置CAN总线通信的比特率为1Mbps。

C610无刷电机调速器采用32位定制电机驱动芯片，高度集成主控、电源和驱动，使用磁场定向控制（FOC）技术，实现对电机转矩的精确控制。同时，C610电机调速器有很高的耐用度，因为其内部设有堵转、过压和断线等多重保护。C610无刷电机调速器功能强大，支持CAN总线指令控制，通过CAN总线获取转子位置和转子转速等信息。

这里选用电机为M2006 P36，它是一种无刷直流减速电机，内含的减速箱减速比为36:1。M2006 P36电机采用三相永磁直流无刷结构，具有控制精度高、输出转速高、体积小、功率密度高等特点。其内部的位置传感器可高精度地反馈位置量，以FOC矢量控制方式使电机产生连续而线性的扭矩，提升控制精密度。

整个系统的硬件接线图如图5所示。

图5 夹持力控制系统硬件接线图

4 夹持力系统软件设计

■4.1 控制系统的主程序设计

机械手夹持力控制系统的控制流程：首先给定一目标速度，在完成系统初始化后，由STM32开发板通过CAN总线向C610无刷电机调速器发送控制指令，电机调速器接收控制指令后控制电机调速器的电流输出，驱动电机运转，使机械手以一定速度慢慢夹持试管，防止速度过快冲击力大对试管造成损坏。同时，电机调速器可以采集电机的位置、转速、力矩信息，将采集的M2006 P36无刷直流减速电机的速度、位置和力矩信息向CAN总线发送，进入CAN中断后进行数据解析，MCU获取电机速度、位置、力矩信息，并将反馈的电机速度信息经控制器按PID控制算法进行运算，产生新的输出并发送新的控制指令给电机调速器，控制电机调速器产生新的电流输出驱动电机运转，机械手夹持试管形成闭环控制。当检测到的电机的力矩达到大于2000时，表明机械手已经稳稳地夹持住试管，这是把目标速度值设置为0，系统就会跟随当前夹持力值，动态地保持现在的状态附近，保持对试管的稳定夹持。机械手夹持力控制系统的流程图如图6所示。

图6 夹持力控制系统主程序流程图

■4.2 控制系统的算法设计

首先进行PID参数的初始化。进行初始化就要定义PID类型结构体，包括模式、输出限幅、积分限幅、Kp、Ki和Kd。

将之前用试凑法得出的Kp、Ki和Kd值赋给变量，PID模式采用增量式，需要进行积分限幅和输出限幅。有时PID计算会出现错误，产生一个过大的值，发生误操作，机构就可能会因输出过高而引发一系列危险事故，PID输出限幅限制了它的最大值，屏蔽了可以出现的误操作，从而避免了这些事故的发生。同理，PID积分限幅也是为了防止积分累计偏差过大而引起的积分饱和现象。设置了积分限幅值后，当积分项将超过额定上限值时，则保持在上限值，避免因积分饱和而失去控制。

在初始化完成后，发送控制指令驱动电机运行时，为了防止电机启动瞬间电流过大，影响PID控制器对当前电机信息参数产生错误的计算，所以要延时一段时间，防止干扰。在电机开始运转后，以给定速度和反馈速度的偏差为输入，按上文提到的增量式PID算法表达式不断计算，以当前采集的速度值为参数，代入pid_calc()函数进行计算，产生新的参数，发送给电机调速器对应的电流信息，驱动电机运转，机械手则不断夹紧，直至达到给定夹持力。同时，在PID计算的过程中，依上文所述，要考虑因特殊情况而产生的误操作，故设置输出限幅和积分限幅，编制对应的程序，保障系统安全运行。

■4.3 控制系统的CAN总线通讯设计

CAN总线初始化目的在于通过参数配置保证CAN总线正常通讯。确认TJA1050当前为复位状态，开始初始化配置。在完成初始化后，TJA1050进入工作模式，就可以正常进行CAN总线数据通讯。初始化步骤为：（1）配置相关引脚的复用功能，使能CAN时钟；（2）设置CAN工作模式及波特率等；（3）设置滤波器及中断优先级等。

CAN节点采用轮询式发送方式，当发现端口有报文时，端口会自动发送。本设计使用标准ID的数据帧，标识符为0x200，数据长度为8字节。本系统最多可以同时控制4个电机，通过字节偏移量的不同区分电机ID，在这里控制ID为2的电机，将电机的设置电流赋值到数据域的第2、3字节，Data[2]存放控制电流值高8位，Data[3]存放控制电流值低8位，STM32中库函数提供了发送消息的函数CAN_Transmit，供发送信息使用。等总线空闲后，则完成发送，发送信息储存在FIFO邮箱中，这时，电机调速器就可以通过CAN协议来接收STM32控制板发送的控制指令报文，将其解析以产生新的控制电流，驱动M2006 P36电机运转，控制机械手夹持试管，接收解析报文由硬件自动完成。

机械手夹持试管时，C610电机调速器通过CAN总线向STM32开发板发送采集到的电机位置、速度和力矩等信息。CAN节点采用的接收方式为中断接收，当正确的CAN数据到来后，启动CAN的中断服务程序，进入USB_LP_CAN1_RX0_IRQHandler(void)的回调函数中，读取中断标志位寄存器，识别帧ID，接收保存电机调速器传送的数据，再关闭CAN中断，最后进行数据处理与解析，进行下一轮PID计算。

电机调速器向总线上发送的反馈数据。报文格式采用标准ID的数据帧，标识符为：0x200 +电机调速器ID，使用的电机ID为2，则标识符为0x202，数据长度为8字节。数据域里每个字节分别存放在对应的电机信息，具体如表1所示。

表1 报文数据域中的电机信息

5 控制系统测试与分析

为了验证该机械手夹持力控制装置的控制效果，搭建夹持力测试系统。首先在机械手上安装压力传感器，再通过A/D转换模块输入STM32控制器，通过USB转UART协议，以串口通信的方式，把传感器检测到的压力值通过串口实时显示。测试系统硬件接线如图7所示。

图7 测试系统硬件接线图

传感器这里选用精度较高的5kg电阻应变式悬臂梁压力传感器。机械手夹持试管时，悬臂梁会产生应变，传感器内部的电桥会将应变转换成电压信号，后续再采用Hx711模块处理得到机械手夹持试管时产生的压力。

24位专用模数转换器芯片 Hx711不仅起到把传感器输出信号放大的作用，而且也负责把压力传感器输出的模拟量信号转化为数字量，具有速度快、抗干扰、集成度高和性能稳定等特点。该模块为两路可选择差分输入，在5V的工作电压下，选择通道A放大128倍来把微弱的模拟量信号转换成数字量并放大，通过管脚PD_SCK和DOUT以串口方式输出数据，完成模数转换，进入开发板。

按照芯片的流程图即可读取夹持力数据，如图8所示。首先对Hx711进行初始化，只有进行初始化后才能根据流程图完成压力信号的读取。一开始数据线管脚Dout电平为高，表明A/D转换器还未准备好输出数据，这时拉低串口时钟线SCK，进入芯片自动复位状态，同时开始延时一段时间。因为Hx711芯片复位后要经过4个数据输出周期后才能稳定地输出数据，进入正常工作状态。当Dout电平由高变低后，SCK应输入25~27个不等的时钟脉冲，因为这里一直选择的是通道A的128放大倍数，所以会输入25个脉冲，SCK每跳变一次，Dout输出一位数据。在24个跳变后输出了24位数据，通过控制SCK的第25个脉冲选择下一次转换放大倍数和通道，这样一个周期的数据就采样完毕。

图8 读取压力传感器数据流程图

机械手夹持试管时产生压力值用下列公式计算可得。其中Yx为目前压力，单位 kg；Dx为直接读取到的Hx711采样数据；D0表示未施加压力时Hx711采样数据；Ymax表示压力传感器最大量程，单位kg；Vc表示Hx711芯片的模数转换器参考电压，单位V；VG表示压力传感器的激励电压，单位V；S表示压力传感器的灵敏度，单位mv/v；A表示Hx711所采用通道的对应放大倍数。

启动夹持力控制系统，机械手开始慢慢夹持目标试管，实验效果如图9所示。同时选择对应串口，配置波特率、停止位、数据位和奇偶校验，完成配置后，点击打开串口，串口开启实时显示当前夹持力。经检测，在给定夹持力为3N的情况下，机械手夹持力稳定在2 95N附近，机械手可以完成对试管的稳定夹持，且未对被夹持物造成夹痕。机械手夹持力值如图10所示。

图9 夹持力控制系统测试效果图

图10 串口显示机械手夹持力

6 结语

在市场上众多机械手需要安装复杂的传感器来保证控制效果，笔者研究的机械手夹持力控制系统结合实际需求，创新地提出一种无外置传感器的恒夹持力机械手系统，通过C610无刷电机调速器获取电机转子位置、转速、输出转矩等信息通过CAN总线发送给STM32开发板，将给定速度与反馈速度的偏差输入控制器按PID控制算法处理，再通过CAN总线发送控制指令给电机调速器驱动电机运转，形成闭环控制。完成系统后，在机械手上安装压力传感器，建立压力检测系统，通过串口的方式实时显示机械手在夹持过程中的夹持力，经检验，这种无外置压力传感器的恒压力机械手系统能稳定地夹持目标物体，保护被夹持物的完好。这种以控制速度间接控制夹持力的方式比直接控制压力更具安全性，直接控制压力方案若在夹持过程中发生意外，试管滑落未放在夹持位置，此时力反馈就会突然变成0，造成极大的偏差量，电机会驱动机械手以很大的速度去夹，直接会损坏机械手结构，造成事故。所以本方案更具有实用性。