2022年12月14日 | 基于STM32的六自由度机械臂控制与PID仿真

为实现六自由度机械臂的控制抓取并在此基础上提高抓取精确度，本文采用D-H法建立基于xArm1S智能总线机械臂的数学模型。并在Matlab2020a的RoboticsToolbox工具箱中编程实现给定机械臂末端始末位置条件下机械臂各连杆的运动仿真和角速度解算。最后以STM32单片机为核心控制模块，实现六自由度机械臂的各连杆运动控制，并在Simulink中搭建模块加入PID控制器对机械臂运动进行仿真。

六自由度机械臂具有效率高、灵活性强等优点，并可取代人类进行各类精密操作，有很强的实用价值和意义。本文选取图1中的xArm1S机械臂进行研究，为了提高精度，在使用前将机械臂固定在桌面。xArm1S机械臂在机械结构上包括六自由度机械臂由底部基座、6个舵机、中间连杆和其他固定机构，基座底部包含固定装置、电源接口和机械臂核心控制电路。针对机械臂底部基座建立空间直角坐标系，以基座底部中央为原点，Z轴方向竖直向上，X轴，Y轴在桌面平面内，X轴沿纸面向外。底部基座可实现机械臂沿Z轴的旋转，关节1、2、3可在Y-Z平面内改变与Y轴的夹角。关节4为关节3末端的抓手，通过舵机连动齿轮转动带动抓手开合从而实现物品抓取，在建立坐标系时可将其视为一个关节。

图1 xArm 1s总线机械臂

1、机械臂运动学分析

连杆坐标系的建立

D-H法的作用是由矩阵变换求算终端抓手与基座坐标系的位置关系。在每个连杆n上建立坐标系[n]，规定坐标原点为an和轴线交点处，Xn坐标轴为Zn-1轴和Zn轴公共垂线方向，Zn轴方向为关节轴线n方向，指向Zn轴偏离Zn-1轴。Yn按照右手螺旋定则确定。图2中为按规定要求建立的坐标系。本机械臂共有1个基座、3个关节和1个末端抓手，基座可实现机械臂在平面内360°旋转，因此针对基座的连杆坐标系Z轴方向在纸面内向上，Y轴和X轴在平面内。对于连接基座和连杆1的关节1，Z轴方向朝纸面向内，X轴和Y轴方向在纸面内。关节1可实现连杆1及以上部分关节在纸面内的270°转动。关节2和关节3的连杆坐标系的Z轴方向同样沿纸面向外，关节4可实现末端抓手的360°旋转抓取，关节5可实现抓手的闭合，实现系列抓取动作。

图2 xArm 1s机械臂连杆坐标系图

2、机械臂运动仿真与角度计算

2.1基于Robtics Toolbox工具箱的机械臂运动仿真

采用Robotics Toolbox10.4工具箱编写程序搭建模型实现了六自由度机械臂的运动仿真，生成GUI界面如图3所示。RoboticsToolbox可实现机械臂的可视化显示、多关节位置和相关参数计算。通过调整按钮q1-q6可依次改变基座和连杆1、连杆2、连杆3、尾部抓手的姿态。同时X、Y、Z可显示机械臂末端的空间坐标。

图3 机械臂仿真界面图

2.2始末位置设定与角度计算

在RoboticsToolbox工具箱中，编写程序并输入规定的机器臂的始末位置，可实现机械臂对应连杆运动角速度和角加速度、机械臂末端轨迹的求解与输出。仿真中设定六自由度机械臂末端的初始位置在原点，结束位置的角度坐标为[pi/4，-pi/3，pi/5，pi/2，-pi/4，pi/6]。图4表示仿真得到此过程各个连杆的相关运动信息，可将其记录并通过实物实现运动过程。

图4 仿真结果图

由工具箱仿真可求出给定始末位置机械臂运行时各角度和角加速度的变化情况。图4中最后一行为机械臂仿真后的终止位置图，在仿真过程中可实时显示机械臂各关节的运动情况。前三行图分别为机械臂末端在坐标系下的位置变化图，第四行和第五行分别为机械臂六个自由度连杆速度和加速度随时间的变化图像。分析图像可知，各关节的运动角速度在运动过程中随时间逐渐提高，于中期到达最大值，之后逐渐减少到0，角加速度在运动前期和运动后期各有一次峰值，体现了各关节的角度变化情况。

3、基于 STM32 单片机的机械臂控制系统

六自由度机械臂系统由电源及调理模块、单片机控制系统、六自由度机械臂机械结构、姿态控制蓝牙手柄、舵机组成，主控芯片为STM32单片机。六自由度机械臂共包含6个舵机，分别为1号总线舵机、5号LX-225舵机、2号3号4号6号LX-15D舵机。舵机均具有堵转保护和过温保护，支持回读，具有温度、电压和位置反馈。供电后通过编程接口将控制程序拷入STM32单片机中，结合时钟电路和复位电路实现舵机控制，从而自由改变机械臂的姿态。本文系统原理图如图5所示。

图5 系统原理图

3.1电机的控制实现

外接电源通过电源器供给系统电压为12V，通过编程接口将机械臂控制程序烧录到单片机内。电源输入后，单片机输出PWM脉宽信号，通过修改输入信号的脉宽可以转换舵机角度。一般在脉冲宽度为1.5ms时，舵机位置归一化。此后，脉宽每增加0.5ms，舵机输出轴角度顺时针旋转45°；脉冲宽度每减小0.5ms，舵机输出轴的角度就逆时针旋转45°。最后，实现了机械手各关节的挠度控制。此外，还可通过蓝牙连接上位机的方式进行机械臂的精确控制。在运行过程中，电位计测量并反馈电机输出轴的位置至STM32单片机，将电机的实际位置与单片机设置的目标位置进行比较，构成闭环控制系统，最终减少偏差，与设定值相匹配。在程序设计上，首先进行定时器的初始化配置，之后复位，并通过设计一定的延时保证动作可以完全执行。六自由度机械臂的舵机控制为定时器控制多路电机，STM32的高级定时器TIM1可产生7路定时器，因此可修改6路通道的比较寄存器值，实现6路舵机的控制。

3.2蓝牙模块的连接与通讯实现

系统选用HC-05蓝牙模块，通过蓝牙模块实现机械臂与上位机之间的通讯，并通过上位机对机械臂各关节的运动状态进行操纵。将HC-05蓝牙模块的波特率选在5000bps。HC-05蓝牙模块在与STM32单片机连接时，Vcc连接电源5V，HC-05的GND引脚与STM32模块的GND引脚相连接，RXD连接STM32模块的TXD引脚，TXD连接STM32模块的RXD引脚。使用HC-05模块时，可以忽略蓝牙的内部通信协议。两台设备共用同一个串口。一个设备通过串口发送数据，另一个设备可以通过串口接收数据。串行通信的实现分为六个步骤：第一，上电和复位；二、GPIO端口模式设置；二是串口参数初始化；第二，中断初始化NVIC；第三，中断函数编码。中断函数主要包括信息的发送和读取两部分。读取数据的函数为uint16_tUSART_ReceiveData（USART_TypeDef*USARTx）。

3.3PID 仿真与优化实现

由于在实际工作过程中会出现一定误差，对六自由度机械臂的每个关节单独引入PID控制，可减少机械臂在工作中受到外部因素的干扰。通过调整比例，可以提高系统的速度和响应峰值时间Tp；通过改变积分，可以提高系统的稳态精度，减小系统的响应误差；通过改变微分，可以提前减小系统响应的超调量，提高系统的稳定性。在机械臂系统前加入PID控制环节实现对机械臂系统的校正，在实际中各个机械臂的传递函数均不相同，一般通过实验输入单位阶跃得到输出结合数学方法求出机械臂的实际传递函数。PID输出系统框图如图6所示。

图6 PID 控制系统原理图

4、创新点

本文包含六自由度机械臂的仿真和控制全过程，可应用至实际项目中并实现多种复杂环境下各关节的控制：首先依据实际工程需要确定机械臂末端的起始位置和末端位置，之后通过RoboticsToolbox工具箱实现机械臂运动过程的仿真，求解得到此过程中各个关节的位置、运动角速度和运动角加速度。之后编写程序采用STM32单片机控制舵机实现机械臂的全部运动过程，同时可验证方程和仿真的准确性。为了弥补PID控制的部分缺点，可在后期对机械臂建立规则引入模糊控制，对于提高模型的精准度有较大意义。

5、结语

本文采用D-H法建立基于xArm1S智能总线机械臂的数学模型。并在Matlab2020a的RoboticsToolbox工具箱中编程实现给定机械臂末端始末位置条件下机械臂各连杆的运动仿真和角速度解算。最后以STM32单片机为核心控制模块，实现六自由度机械臂的各连杆运动控制。引入PID控制进一步实现优化。