基于STM32的机械手夹持力控制系统设计_刘琪.pdf

wwwele169com|11电子电路设计与方案0 引言在日常生活中，机械手通常用于夹持物体，很多机械手采用开环控制，这样就导致当机械手夹持力过大时，被夹持物破裂，影响其使用，当夹持力太小时会导致机械手夹持松弛，不能稳定夹持以进行自动化作业。以试管为例，试管属于材质比较脆弱的物品，常用于实验，需要对夹持力精准地控制，不能在夹持过程产生大的超调，不然可能会夹碎试管，所以能很好的测试系统效果。夹持时由于试管会产生形变而导致夹持力变化，所以应实时检测试管所受压力值，不断调整控制输出以保证稳定夹持。而且无外置传感器的方式，在实际应用中会更实用，更好推广，不需要在机械手上安装复杂的传感器，简化了安装。1 系统整体方案夹持力系统是靠控制无刷直流电机的转速来控制机械手的夹持力，控制的夹持力与电机的转速呈一定比例关系，所以本系统通过构建一个速度闭环来间接控制机械手稳定夹持试管。整个系统主要由机械手、STM32 开发板、电机调速器、无刷直流电机几部分组成。首先给定一个目标速度值，机械手开始慢慢夹持试管，电机调速器将检测到的电机位置、速度和力矩信息通过 CAN 总线反馈到输入端，这时给定速度与反馈回来的速度的差值输入控制器，控制器按 PID 控制算法处理输入信号并产生输出，通过 CAN 总线发送控制指令给电机调速器，从而控制电机驱动机械手夹持试管，产生新的速度值，形成闭环控制。系统框图如图1所示。2 系统数学模型及仿真本设计采用的 PID 控制器，它的传递函数表示为：()()pppdiKU sKK T sTsE s=+根据直流无刷电机的数学模型，其传递函数为：()()()22taadLaaetaaetKrL ssUsTsL Jsr Jsk KL Jsr Jsk K+=+则本夹持力控制系统对应的数学模型如图 2 所示。PID控制主要包含3个控制参数：P（比例）、I（积分）、D（微分），根据搭建的系统，对输入信号处理，实现无偏差输出。本设计中选用增量式 PID 控制策略。由下面公式可知，增量式算法只与最近 3 次偏差值有关，工作量小，不仅能够保证很好的实时性和快速性，而且不会产生很大的累积偏差，对系统影响较小，出现意外还可以输出限幅和积分限幅等方式限制本次输出。增量式 PID 的表达式：()()()()()()()()()1 212pidu kKerr kerr kK err kKerr kerr kerr k=+其中：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 所示。整基于 STM32 的机械手夹持力控制系统设计刘琪（江苏安科瑞电器制造有限公司，江苏无锡,214400）摘要：为保证被夹持物品的完好以及机械手夹持的稳定性，提出了基于STM32的机械手夹持力闭环控制系统，该系统主要解决了在机械手夹持过程中夹持力控制问题。采用PID为控制策略，CAN总线为通讯方式，创新地提出无外置传感器的方式，实现机械手夹持力闭环控制。关键词：机械手；夹持力控制；CAN总线；嵌入式控制器电机给定速度+-机械手电机调速器图 1 夹持力闭环控制系统框图r(t)-c(t)-+图 2 夹持力控制系统数学模型DOI:10.16589/11-3571/tn.2023.01.00912|电子制作 2023 年 1 月电子电路设计与方案个过程在开始经历了少量的超调后，能够较快的稳定在给定值 3N 附近。在整定的过程中，加快响应速度往往以提高超调量为代价，综合考量后，采用第一组 PID 整定参数，在保证超调量不高的情况下，保持一定的响应速度，提高了系统整体的性能。01234567891000.511.522.533.54PID 1PID 2PID 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 所示。STM325VGNDPA11PA12TJA1050VCCGNDCANHCANLTXRXC610无刷电机调速器CANHCANL电源线+电源线-24V5VM2006 P36直流无刷减速电机位置传感器数据线位置传感器数据端口三相动力接头三相动力线图 5 夹持力控制系统硬件接线图4 夹持力系统软件设计 4.1 控制系统的主程序设计机械手夹持力控制系统的控制流程：首先给定一目标速度，在完成系统初始化后，由 STM32 开发板通过 CAN 总线向 C610 无刷电机调速器发送控制指令，电机调速器接收控制指令后控制电机调速器的电流输出，驱动电机运转，使机械手以一定速度慢慢夹持试管，防止速度过快冲击力大对试管造成损坏。同时，电机调速器可以采集电机的位置、转速、力矩信息，将采集的 M2006 P36 无刷直流减速电机的速度、位置和力矩信息向 CAN 总线发送，进入 CAN 中断后进行数据解析，MCU 获取电机速度、位置、力矩信息，并将反馈的电机速度信息经控制器按PID控制算法进行运算，产生新的输出并发送新的控制指令给电机调速器，控制电机PID(s)PID Controller50.0054s +0.18s+12Transfer FcnScopeStepPID(s)PID Controller150.0054s +0.18s+12Transfer Fcn1PID(s)PID Controller250.0054s +0.18s+12Transfer Fcn2图 3 夹持力控制系统仿真建模wwwele169com|13电子电路设计与方案调速器产生新的电流输出驱动电机运转，机械手夹持试管形成闭环控制。当检测到的电机的力矩达到大于 2000 时，表明机械手已经稳稳地夹持住试管，这是把目标速度值设置为0，系统就会跟随当前夹持力值，动态地保持现在的状态附近，保持对试管的稳定夹持。机械手夹持力控制系统的流程图如图 6 所示。开始给定速度值接收电机调速器采集的位置、速度和力矩信息向电机调速器发送电流数据当力矩信息大于2000时给定速度值为0PID控制器调节输出是否 图 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 的数据帧，标识符为0 x200，数据长度为 8 字节。本系统最多可以同时控制 4 个电机，通过字节偏移量的不同区分电机 ID，在这里控制 ID为2的电机，将电机的设置电流赋值到数据域的第2、3字节，Data2 存放控制电流值高 8 位，Data3 存放控制电流值低 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 的数据帧，标识符为：0 x200+电机调速器 ID，使用的电机 ID 为 2，则标识符为 0 x202，数据长度为 8 字节。数据域里每个字节分别存放在对应的电机信息，具体如表 1 所示。表1