基于红外定位的六自由度板球控制系统设计①

王正刚， 代广珍

(安徽工程大学电气工程学院，安徽 芜湖 241000)

0 引 言

板球控制系统是一种多变量的非线性控制系统，是杆球控制系统的扩展和延伸[1]。板球控制系统可以很直观地验证控制算法的有效性，为实际工程上的控制问题提供解决方案。同时，研究板球控制算法可以提高实际系统的稳定性。因此，设计一种合理有效的板球系统控制方法是非常有意义的[2]。

系统以2017年全国电子设计大赛B题为背景，设计并搭建了一个六自由度运动控制平台，采用STM32F407单片机作为主控模块，包含角度检测模块、直线电机模块、红外线定位装置、电机驱动和直流稳压电源模块，通过采集获取平台的姿态参数并进行计算调节，实现对小球轨迹的控制，通过控制平台的倾斜，使直径不大于25mm的小球能够按要求在限定时间内完成各种指定的运动轨迹。

1 方案的选择

1.1 滚球控制方案

小球运动平台的姿态调整可采用多自由度并联控制，利用PID控制算法进行运算，并将运算结果用于平台控制。最简单的控制方式是在x和y轴上对平台进行上下调整的二自由度控制。六自由度控制设计虽然复杂，但位置控制精度高，图1为六自由度并联运动平台设计效果图。六个动力系统平均分布在平台下方，与其他方案相比，六自由度并联运动平台具有刚度大、位置精度高和承载能力强等优点，可实现对平台的精确控制。

图1 六自由度并联控制运动平台设计效果图

图2 铰接点矢量图

1.2 硬件设计方案

使用直线电机，结构设计紧密，重量轻，用直流电机齿轮减速，内置微动开关，可实现行程走完自动停止，安装简易，同时易于调节和控制。采用MPU6050传感器。MPU6050模块内部自带电压稳定电路，直接连接STM32F407总控制器系统。通过读取MPU6050内部运动处理器所计算出的结果，可解算得到姿态角，读取简单、响应迅速，可有效减轻CPU负担。通过设置红外定位装置，检测小球的位置，反馈到控制器，计算控制量，通过控制直线电机以控制平台的倾斜角度，进而控制小球的运动轨迹。

图3 系统总体框图

图4 MPU6050原理图

2 运动平台位置计算分析

已知运动平台的姿态参数为q=(x,y,z,α,β,γ)，利用位置反解方法求解伸缩杆的杆长li。图2为六自由度并联运动平台的铰接点的矢量图，如图2所示A为固定平台，B为运动平台，A上对应铰接点Ai与B上对应铰接点Bi与之间的距离|AiBi|即为各个伸缩杆的杆长li。

T为旋转变换矩阵，反映了固定坐标系OXYZ和运动坐标系ouvw之间的关系。p=(xpypzp)T是运动坐标系原点o在固定坐标系中的位置矢量ai=(xAi,yAizAi)T是Ai在固定坐标系中的位置矢量；bi=(xBiyBizBi)T是Bi在运动坐标系中的位置矢量。由图2可知，伸缩杆的杆长矢量Li为：

ai-(T×bi+p)

(1)

因此，计算伸缩杆长度的公式为：

li=|Li|=ai-(p+T×bi)=

(2)

已知ai与bi，给定运动平台B的位姿参数q=(x,y,z,α,β,γ)，通过位置反解，就可以确定向量p与矩阵T，再利用公式(2)就能算出伸缩杆长度li[3]。

3 系统设计

3.1 电路设计

3.1.1 系统总体框图

图3所示为系统总体框图，主要由STM32F407单片机、MPU6050六轴传感器模块、直线电机控制模块、红外定位装置和直流稳压电源模块等部分组成[4]。

图5 直线电机驱动原理图

图6 红外定位装置示意图

3.1.2 角度检测模块设计

角度检测模块采用MPU6050六轴传感器设计，图4所示为MPU6050设计原理图。MPU6050内部自带三轴陀螺仪和三轴加速度传感器，含有两个I2C接口，主I2C接口结合自带的数字运动处理器硬件加速引擎，可以向应用端输出完整的九轴姿态融合演算数据；从I2C接口可以和外部磁力传感器连接。使用运动处理资料库，可非常方便地实现姿态解算，降低主控模块的运算负荷，同时大大降低了开发难度。

3.1.3 直线电机驱动设计

IR2104型半桥驱动芯片可以驱动高端和低端两个N沟道MOSFET，能产生较大的栅极驱动电流，并具有硬件死区、硬件防同臂导通等功能。使用两片IR2104可以组成完整的直流电机H桥式驱动电路。电机驱动原理图如图5所示[5]。

图7 程序流程图

3.1.4 红外定位装置设计

图6所示为红外定位装置示意图。在平台四周放置红外线发射和接收阵列，系统逐个扫描光电管接收的信号。因为小球的阻断，通过检测未接收到信号的光电管便可以快速获取横向和纵向坐标，从而确定小球在平台上的精确位置，定位精度达到5.4mm[6]。

3.2 程序设计

3.2.1 程序流程图

图7所示为系统控制程序流程图，包含主程序流程和中断程序流程。主程序主要完成功能是对系统进行初始化和延时UI(用户界面)处理，包括OLED实时显示小球位置区域及停留时间、按键控制等；中断处理程序主要是对平台姿态(加速度、角速度)、小球位置和速度、直线电机长度和速度等数据进行获取和处理，控制输出PWM信号，以及对小球进行定位时间检测等。

3.2.2 程序功能分析

系统通过STM32F407单片机来控制驱动电路，根据PID的调节来得到需要设定的角度，同时得到对应控制脉冲，输出PWM控制直线电机的推杆伸缩，使平台按照指定角度倾斜，从而使小球按照指定动作运动。其中按键开关进行模式和区域设置。

4 测试方案及结果

4.1 测试设备及方式

所用测试设备有：小球、量角器、直尺、秒表等。

测试方法描述如下：

(1)定位测试。将小球放置在平台上指定区域内，保持一定时间静止。

(2)定点测试。将小球放置在平台上某区域内，调节滚球控制系统使小球移动到另一指定区域内，并停留一段时间。

(3)定时测试。使小球在某一规定时间内完成指定动作。

4.2 测试数据

(1)驱动滚球控制系统工作，使小球在规定区域内停留，并记录停留时间和偏差位置。测试结果如表1所示：

表1 区域定点停留测试数据

测试结果分析：定点停留测试能够准确完成，误差极小。

(2)驱动滚球装置系统工作，使小球从一个区域滚动到另一个指定区域，并停留一段时间。记录运动时间和停留时间。测试数据如表2所示：

表2 运动和停留测试数据

测试结果分析：运动停留测试基本能够准确完成，但是在路线复杂情况下，运动时间较长。

(3) 规定时间内，让小球先后进入区域 2、区域 6，停止于区域9，在区域9中停留时间不少于2s。记录运动时间和停留时间，测试数据如表3：

表3 规定路线测试数据

测试结果分析：要通过的区域多，还要在指定区域停留，因此实现起来有一定难度，但是基本可以在指定时间内完成要求。时间上还有待缩短。

(4)用按键设置A、B、C、D区域，并控制小球完成动作。

表4 按键指定区域测试数据

测试结果分析：设置区域路线简单，能够较好完成，但是若为折线路线，则实现起来比较困难，在运动过程中，难以控制，耗时较长。

5 结 论

系统搭建了基于红外定位的六自由度板球控制平台，使用STM32F407单片机作为主控模块。经测试，控制精准，操作简单，性能可靠，技术指标达到了设计要求。

在系统设计和搭建过程中，分工明确，充分发挥各自所长，放弃使用传统的舵机作为执行机构，采用六自由度并联控制运动平台，虽然设计难度有所增加，但提高了控制精度，可实现对平台的精确控制。

[1] 苏信, 孙政顺, 赵世敏.板球系统的模糊控制方法研究[J].计算机仿真. 2006(09).165-167.

[2] 翟晨汐,李洪兴. 板球系统的直接自适应模糊滑模控制[J].计算机仿真. 2016(02).383-388.

[3] 李沛,杨小强,李焕良,等.基于Matlab/SimMechanics的六自由度并联运动平台建模与分析[J].机械与电子.2016(03).75-80.

[4] 王红睿,田彦涛,隋振,等.板球系统的非线性自适应控制[J].系统仿真学报.2010(05).1251-1256.

[5] 王金晨,纪成,何印洲.基于PWM的可调速电动汽车模型控制系统的设计[J].伺服控制.2011(08).42-45.

[6] 何成林,钟鸣,姚玉峰. 基于红外定位的微小型自重构移动机器人[J]. 机械与电子.2016(09).69-72.