刘以行 叶仕通 何伟浩 邓嘉豪

(广东工业大学华立学院, 广东511325)

可控高空作业机械臂控制系统设计

刘以行 叶仕通 何伟浩 邓嘉豪

(广东工业大学华立学院, 广东511325)

目前城市化园林在自动化建设和高空作业方面设计出可高空作业机械臂控制系统。机械臂控制系统采用STM32F103RCT6芯片为控制核心，运用电机PID控制器、姿态平衡传感器和三杯式风速传感器三个核心部件组成的传感器控制系统和机械驱动控制，实现机械臂作业反馈和智能避险功能。实验证明，控制系统与臂体融合程度高，具有良好的控制性能。

高空作业机械臂；传感器控制；电机控制

在城市园林管理及绿化建设的过程中，传统的人工直接手持锯木设备对城市立着的树木进行修剪是一件危险且又辛苦的工作，而高空作业机械臂的出现弥补了园林建设、城市绿化这一缺陷，减少修剪人工的使用，降低该建设过程的成本；同时提高了修剪效率，其自动化的控制完全不需要考虑到人工修剪时会出现的疲劳现象；其控制系统自带安全调控应急程序，有效地降低了伤害几率。尤其是目前城市道路两旁的大树数量大，且绝大部分栽种的树木是生长速度较快，枝干容易长长的品种，因此常需要修剪。所以高空作业机械手臂的市场发展前景还是相当可观的。高空作业机械臂的控制系统的优秀与否是衡量是否具有实用价值意义的关键因素，其中可靠的安全反馈机制是非常必要的，机械臂控制系统采用STM32F103RCT6芯片为主控核心，结合机械结构特点，配合由风速传感器和MPU9250传感器模块构成的传感器反馈系统，使用蓝牙模块、wifi模块和串行通讯方式作为上位机与下位机之间的通信桥梁，最后各部件联动控制，实现整个机械臂的控制。系统结构图如图1所示。

图1 系统结构图

1 系统硬件组成

机械臂控制台搭建以32位ARM3微处理芯片STM32F013RCT6为微控制核心，由12 V的直流电供电，由AMS1117-3.3芯片和线性稳压芯片LM2940共同集成供电模块和由两块BTN7970构成的全桥电路和一块74lS244芯片构成的保护电路组成的电机驱动模块。具有电源转换效率高，噪音低，抗干扰能力强等优点。机械结构主要采用质量相对较轻的铝合金材料制造，适用于不同场合使用时的安装与拆卸。机械臂底座根据情况采用皮带固定或螺栓固定，使用步进电机为机械臂提供旋转动力。在手臂与底座以及大臂与小臂的连接处则利用减速电机提供手臂的摆动动力，大臂与小臂都采用方形结构，使手臂具有良好的抗扭性能及较大的抗弯刚度。在小臂中提供一组动力，控制手掌的旋转。机械臂抓取部分采用两指结构，使用MG996R舵机提供动力，利用滑块机构连接并控制两指的运动。

2 控制系统

2.1 电机驱动控制

电机组采用BTN7971驱动芯片的直流电机驱动板，双BTN7971驱动芯片及附属电路形成一个 “H桥驱动电路”，便于开发人员控制驱动电机需要使能信号线ENABLE，两个方向信号线DIR-L和DIR-R。H桥驱动电路图见图2。

图2 H桥驱动电路图

通过信号线控制电机的转速和角位移量，其中H桥电路必须异侧导通，当DIR-L信号为0，DIR-R信号为1，当ENABLE为1时，电流从左往右流通，反之电流反向流通，通过控制电流在电机中的流向实现电机组的正反转控制，伺服电机部分增加反馈信号线实时传输转速和位置数据，从而可以根据实际情况配合相关传感器对电机组进行更精确地控制。信号线端使用脉冲宽度调制(PWM)输入，通过设置STM32芯片的定时器，设置TIMx_CCER、TIMx_CCRx等寄存器的相关标志位，就能产生PWM输出，并进行脉宽在频率和占空比上的控制。脉宽和周期测量时序图见图3。

图3 脉宽和周期测量时序图

2.2 电机PID控制器

使用PID控制器是较为有效和稳定的控制方式，参数易调，控制简单，广泛应用于机器人、平衡小车等自动化控制领域。

(1)

(2)

由式(1)和式(2)可得增量式PID控制算式：

(3)

式中，ΔUk为第k次采样时刻的计算机输出值；Kp为比例系数；KI为积分系数；KD为微分系数；ek为第k次采样时刻输入的偏差值；ek-1为第k-1次采样时刻输入的偏差值；u0为开始进行PID控制时原始初值。基于式(1)和式(2)得到的增量式PID控制算式具有计算量少的优点，所以在本控制系统设计中，使用增量式PID对伺服电机的速度、位置进行控制。

2.3 姿态平衡控制

机械臂必须具有维持自身平衡的能力，良好的受力点能最大限度地发挥出其动力模块效能。MPU9250模块由两部分组成，一组由3轴加速度计及3轴陀螺仪组成，另一组由AKM公司的AK8963 3轴磁力计组成，并采用I2C协议进行通信，最高可达3.4 Mbps的速度，能很好地帮助机械臂处于自身平衡状态，模块集成了陀螺仪和加速度计，以机械臂重心为中心建立空间直角坐标系X,Y,Z轴，反馈当前机械臂姿态数据，模块的姿态运动学方程作为滤波的状态转移方程，加速度信息作为滤波的观察量信息，结合PID算法和卡尔曼滤波的计算方法，迭代计算更新参数，调整PWM脉宽输出，达到调整电机和舵机角度的目的，使臂体在未受自然因素影响时保持平衡。PID控制系统图如图4所示。

图4 卡尔曼滤波器的PID控制系统图

2.4 上位机监控

本系统采用基于C++的QT框架，使用QT4编写的监控上位机，可跨平台使用。臂身两侧装有监控摄像头，用于图像采集，同时打开两侧摄像头，使用WiFi模块通过802.11n协议实时传送画面至上位机，根据两侧画面合成单幅图像，实时观察操作对象与机械臂的实际情况。同时，风速传感器、PID控制器数据等实时数据会通过特定的通讯方式，经过下位机处理，输送到上位机反馈显示。监控上位机反馈显示如图5所示。

2.5 风速传感器

图5 监控上位机反馈显示

图6 风速传感器结构尺寸图

图7 测量反馈程序流程图

机械臂工作于高空中，对臂体安全性影响最大的自然因数是风速，风速随高度的逐渐变化呈指数上升，危险系数逐渐加快，呈上升趋势，所以机械臂需要一个预报警机制，该系统采用YGC-FS型三杯式风速传感器作为预报警机制的核心部件，其结构尺寸如图6所示。

测量范围为(0～70)m/s，分辨率为0.1 m/s，准确度为±(0.3+0.03V)m/s，启动风速小于0.5 m/s，输出频率信号F代表风速V，计算公式为：

(1)

(2)

三杯式风速传感器由3 个处于同一平面、互成120°的半球形空杯组成。当风杯受到水平风向的驱动时，产生霍尔效应，经过传感器内部处理收集到的数据，通过STM32的DMA 通道实现内存风速数据与 USART 外设的数据传输，并使用RS485串口标配通信协议传送实时数据至上位机，程序中设置预警标志位，当风速超过或达到预警线(预警标志位置1)时，机械臂发出警报并停止工作，马上启动紧急回缩机制，收回机械臂，最终由操控人员决定是否完全关闭机械臂，实现过高风速下的安全保护功能。 测量反馈程序流程图如图7所示。

3 仿真实验

使用仿真软件对控制系统进行进一步的验证和调试。如图8所示。在没有接收到预警信号前，脉冲处于平稳状态，可知电机正常工作，机械臂处于稳定上升状态。由图9可知，模块电压与风速呈负相关关系，随着风速的不断提高，当风速超过或达到预警线时，风速传感器发出信号，检测模块接受到一个高电平脉冲，电机正转脉冲与反转脉冲状态置换，电机开始减速反转，一定时间后稳定工作，机械臂将逐渐下降，可知预警反馈模块运作正常，控制系统基本达到预期结果。

a—电机正转脉冲 b—传感器电平脉冲 c—电机反转脉冲

4 结论

本文提出了一个以高空作业机械臂为主体的控制系统设计，经初步实践，机械臂与控制系统配合程度基本达到设计要求。与同类机械臂相比，该控制系统由传感器模块搭建组成较为完善的反馈机制，传感器模块和驱动控制模块良好融合，使机械臂能在高空环境中稳定工作。现阶段已实现了机械臂与控制系统的初步融合，还需继续深入和完善本机械臂控制系统设计，在机械结构上仍需合理地改进，增添方便拆装的独立电源供电模块及无线传输控制模块等。

图9 风速与模块电压关系图

[1] 马文斌,杨延竹,洪运. 步进电机控制系统的设计及应用[J].电子技术应用, 2015,41(11):11-13．

[2] 李长才,肖金球,张少华．基于 STM32 的风速监测系统设计[J].微型机与应用,2016,35(6) : 91-93.97．

[3] 孟祥萍,张本法,苑全德. 自适应扩展卡尔曼滤波器在移动机器人定位中的应用[J].计 算机系统应用,2015,24(12)．

[4] 韩东,韩冰,郑卫刚. 轨道式焊接机器人工作站在压力容器焊接中的设计[J]．中国重型装备:2015,7(3):28-30．

[5] 杨杰,王瑞．基于加速度输入的无人机航迹 PID 控制[J]. 自动化技术与应用: 2015,34(12)．

[6] 彭圣明,裴海龙,王清阳.基于机械臂的运动学研究与应用[J].机械设计与制造，2011(7) : 136-138．

[7] 李安迎，邓靖靖，邓世建，等. 基于三杯式风速传感器的风速监测站设计[J]. 电测与仪表,2010,47(7A)．

编辑 陈秀娟

Design on Control System of Controllable Manipulator in Aloft Work

Liu Yixing, Ye Shitong, He Weihao, Deng Jiahao

The control system of manipulator in aloft work has been designed for automation construction and aloft work of urbanization garden. The control system of manipulator has been adopted STM32F103RCT6 chip as the control core, and has been used the sensor control system consisted of three core components of motor PID controller, posture balance sensor and three cups of wind speed sensor and has been controlled by mechanical drive, which can realize the manipulator operation feedback and intelligent hedging function. The experimental results show that the control system has a high degree of integration with the arm body and it has good control performance.

manipulator in aloft work; sensor control; motor control

2016—10—11

“攀登计划”广东大学生科技创新培育专项资金资助一般项目，编号PDJH2016b0932；大学生创新创业训练计划项目，编号201613656001

刘以行 (1996—)，男，本科，主要从事自动化控制技术。

TP242.6

A