基于STM32的混凝土激光整平机水平控制系统设计

秦海鹏 张帅 黄欢 曹竞豪

摘  要： 研究一种基于STM32的激光整平机控制系统的设计方案。该系统由STM32微处理器、惯性传感器（MPU6050）、电推杆驱动器等部分组成。以STM32F103处理器作为姿态解算单元，以STM32F407处理器作为控制单元，两者之间通过RS 232接口进行通信。采用扩展卡尔曼滤波算法将MEMS陀螺仪、加速度计和磁力计的测量数据进行融合，利用四元数坐标变换解算出所需的姿态角。搭建一套整平机实验平台用来验证方法的有效性，利用PID算法实现整平机水平倾角的调平控制。实验结果显示，控制系统可以稳定运行，姿态解算偏差不超过0.5°，水平控制时超调量较小，调节时间在2 s以内。综上所述，该系统测量精度高，控制稳定性好，能够较好地实现整平机的水平控制，因此，该系统可以提高整平精度，降低整机成本，具有很高的实用价值。

关键词： 激光整平机; 混凝土整平; 数据融合; 调平控制; 控制系統设计; 有效性验证

中图分类号： TN876?34; TP202                   文献标识码： A                      文章编号： 1004?373X（2020）03?0150?04

Design of concrete laser leveling machine horizontal control system based on STM32

QIN Haipeng， ZHANG Shuai， HUANG Huan， CAO Jinghao

（School of Electrical Engineering & Automation， Jiangsu Normal University， Xuzhou 221000， China）

Abstract： A designing scheme of laser leveling control system based on STM32 is proposed. The system consists of the STM32 microprocessor， the inertial sensor （MPU6050） and the electric pushrod driver. The STM32F103 processor is used as the attitude calculation unit， and the STM32F407 processor as the control unit of the system. The two units communicate through the RS 232 interface. The measured data of MEMS gyroscope， accelerometer and magnetometer are fused by the extended Kalman filtering algorithm， and the required attitude angle is solved with the quaternion coordinate transformation. A series of leveling machine experimental platforms are built to verify the effectiveness of the method. The leveling control of horizontal angle of the leveling machine is realized by the PID （proportion?integration?differentiation） algorithm. The experimental results show that the control system can operate stably， the attitude solution deviation does not exceed 0.5°， the overshoot for horizontal control is relatively small and the adjusting time is within 2 s. In summary， the measurement accuracy of the system is high， the control is stable， and the system can better realize horizontal control of the leveling machine. Therefore， the system can improve the leveling accuracy and reduce the cost of complete machine， thus it has high practical value.

Keywords： laser leveling machine; concrete leveling; data fusion; leveling control; control system design; effectiveness verification

0  引  言

平地铲的水平控制是整平机控制系统中的关键技术，角度传感器为水平控制系统提供平地铲当前的水平倾角，而高精度的姿态角信号是保障系统性能的前提。基于重力摆和阻尼油结构的倾角传感器由于测量误差较大，无法保证系统的精度[1]。随着微机电系统和数据融合算法的发展，MEMS惯性传感器由于体积小、成本低、抗干扰能力强、数据可靠性高等优点，被广泛应用于惯导、航姿领域[2]。陀螺仪和加速度计在积分时间漂移和测量噪声的影响下会产生较大误差[3?4]。为了提高整平后的平整度和水平度，本文通过扩展卡尔曼滤波融合MEMS陀螺仪、加速度计和磁力计的数据，从而得到姿态角的估计值。同时，随着整平机应用领域越来越广泛，需要实现较强的人机交互功能，而传统的8/16位处理器无法满足要求，因此本文采用STM32系列处理器作为姿态解算模块和控制模块。为了模拟整平机在实际运行过程中的各种干扰，搭建一套实验平台并在其上完成了控制算法的设计及其相关试验。

1  整平机工作原理

激光整平机机械结构如图1所示，传统的激光整平机有两个激光接收器[5]，由于激光接收器的价格比惯性传感器昂贵，所以本设计创新性地采用惯性传感器替代一侧激光接收器的方案。整平机机械结构主要由刮平板、平地铲、激光接收器、惯性传感器、水平调节推杆、高程调节推杆及拖拉机组成。水平调节推杆通过四联机构连接于刮平板，高程调节杆直接连接于机身，惯性传感器固定于刮平板一侧，刮平板与平地铲通过连接机构相连。高程调节推杆一般采用手动方式根据水泥厚度实现粗调。平地铲整平作业时会前后、左右倾斜，因此激光整平机自动调节分为两种方式：俯仰角控制和左右水平控制。整平机水平调节时，水平调节推杆通过激光偏差信号实现高程精调，并利用惯性传感器使得平地铲维持在水平位置[6]。

2  总体设计

通过比较各种整平机控制系统的性能优劣，本文中整平机水平控制系统结构如图2所示，主要由惯性传感器、姿态解算控制器、整平机控制器、平地铲组成。其中，姿态解算控制器通过I2C总线读取陀螺仪、加速度计和磁力计的数据，通过扩展卡尔曼滤波算法进行数据融合，为整平机水平控制系统提供实时倾角测量。执行机构为电推杆，采用H桥电路作为电机驱动模块，通过PWM信号控制。

2.1  姿态解算模块

角度测量模块由MEMS陀螺仪、加速度计和磁力计组成[7]，本系统选择MPU6050作为测量水平倾角的惯性传感器。MPU6050是用于惯导、航姿领域的整合性6轴处理模块[8]。该芯片具有I2C通信接口，用于数据交互和外接磁力计。磁力计的型号为HMC5983，其内置三轴磁阻传感器，能在±8高斯的磁场中实现2毫高斯的分辨率。提供I2C通信接口和12位ADC转换器。此外，它具有偏置和温度补偿功能，抗干扰能力强。选择意法半导体公司（ST）的以ARM Cortex?M3为内核的STM32F103TB作为姿态解算处理器，它是市场上32位处理器中功耗较低的产品，含有两个I2C接口及USART接口[9]，符合本系统的要求。

2.2  整平机控制器

整平机控制器以STM32F407为核心，该芯片是基于ARM的32位Cortex?M4内核的通用MCU，具有低成本、低功耗、高性能的特点。STM32F407接口资源丰富，可扩展性强，工作频率最高可达168 MHz，具有哈佛总线结构[10]。它不仅能提供高速的2通道12位ADC转换器，还能提供CAN，I2C，SPI，USART等总线，符合本系统的要求。

2.3  驱动模块

H桥是常用的直流电机驱动电路，主要由4个开关管和1个直流电机组成[11]。根据不同对角线上2个开关管的导通情况，电流能够双向通过直流电机，从而实现电机转向的切换。开关管的导通和关断时长由PWM波的占空比决定，通过调节占空比可以控制电机的转速。为了防止电机转向切换时过大反向电动势损坏开关管的情况，采用4个反并联二极管进行保护。

3  算法设计

3.1  姿态解算

四元数[q（t）]满足以下微分方程 ：

通过陀螺仪的测量数据可以获得系数矩阵[Ωb]：

式中[gx]，[gy]，[gz]为陀螺仪输出的角速度。为了补偿陀螺仪的漂移误差，一般式（1）为状态方程，以加速度计和磁力计的输出为观测量，利用卡尔曼滤波融合三种传感器的数据[12]。

令导航坐标系下的重力向量和磁场向量分别为[00gT]和[bx0bzT]。其中，[g]为重力加速度，[bx]，[bz]为当地磁场强度。则：

式中：[Cnb]为载体坐标系转换到导航坐标系的姿态矩阵，其元素可以表示为四元数的函数;[Cnb*]为姿态矩阵[Cnb]的转置;[ax]，[ay]，[az]是计算得到的加速度在三轴上的分量;[mx]，[my]，[mz]是计算得到的磁场强度在三轴上的分量。设加速度计的实际输出为[a1]，[a2]，[a3]，磁力计的实际输出为[m1]，[m2]，[m3]，则观测误差为[T]：

利用观测误差补偿四元数：

式中：[q-]为补偿前的状态变量;[q]为补偿后的状态变量;[K]为卡尔曼增益矩阵。最终输出的姿态角可由更新后的四元数得到[12]。

3.2  增量式PID控制算法

数字PID控制算法有两种表达式，其中，位置式PID的输出量由此刻设定值与当前值的偏差、历史累计偏差之和及最近两次偏差之差获得[13?14]。设位置式PID的第[k]次输出为[Uk]。

式中：[KP]为比例系数;[KI]为积分系数;[KD]为微分系数;[Ek]为第[k]时刻设定值与当前值的偏差。

由于位置式PID计算了历史累计偏差之和，容易产生较大的累计误差。而增量式PID输出量的大小只与最近三次的偏差有关，是一种递推式的算法，计算误差对控制量的影响很小，因此增量式PID的应用更加广泛。增量式PID的第[k]次输出记为[ΔUk]：

参数整定时，先整定参数[KI]，其大小影响了系统的响应速度，再整定参数[KP]，其大小反映了系统对超调的限制。

4  实  验

开发环境选择MDK5，在自主研发的整平机实验平台上进行姿态解算实验和PID控制实验。

4.1  姿态解算实验

将姿态解算模块的输出与工业级航姿参考系统（北微传感BW?AH200）的输出进行对比，BW?AH200采用高质量的MEMS加速度计、陀螺仪和磁力计，严格的密封设计保证在恶劣环境下仍可以准确测量载体的角速度、加速度和姿态等信息。姿态角分辨率达0.01°，误差精度在1°以內。实验过程中，每隔5 s将两者同时旋转一定角度。多次记录测量数据，实验结果如图3所示。

图3显示，BW?AH200的测量误差一般不超过0.2°，而MPU6050测量的姿态角度与实际角度的偏差最大为2.8°，其余都在2.2°以内。整平机实际工作时，平地铲水平倾角变化范围一般在-5°～5°，由图3可知，偏差最大不超过0.25°，对于2 m的平地铲来说，高度调节误差可控制在0.4 cm以内，满足实际要求。

4.2  PID控制实验

本课题组设计了一套整平机控制系统实验平台。采用模拟路况推杆来模拟整平机在实际工作状态下平地铲的水平倾角扰动。实验平台如图4所示。

图4中，1为俯仰角调节电动推杆，2为模拟路况电动推杆，3为左右水平调节电动推杆（2个），4为24 V直流稳压电源。对刮平板进行水平控制实验，为了测试不同参数下的水平控制效果，在给定阶跃扰动下观察刮平板的姿态角变化。首先调节参数[KI]，系统的响应如图5，图6所示。

当[KI]=300时，姿态角进入稳定状态所需时间为7 s左右;当[KI]=600时，姿态角进入稳定状态所需时间为3.2 s左右。由图5，图6可知，[KI]越大，系统响应速度越快。

调节[KP]以减小控制过程中存在的超调，系统响应曲线如图7所示，参数[KP]=500，[KI]=600。由图6，图7可知，超调量降低了约0.4°。

5  结  论

本文基于STM32设计了一套激光整平机水平控制系统，利用扩展卡尔曼滤波实现了惯性传感器的数据融合，完成了刮平板调平实验，并比对了不同参数下PID的控制效果，控制系统可以稳定运行。为了实现精度更高的动态响应，在以后的研究中将采用其他精度更高、抗干扰能力更强的角度传感器，利用非线性、自适应等控制算法实现高精度控制。

注：本文通讯作者为黄欢。

参考文献

[1] 李庆，罗锡文，汪懋华，等.采用倾角传感器的水田激光平地机设计[J].农业工程学报，2007，23（4）：88?93.

[2] 李荣冰，刘建业，曾庆化，等.基于MEMS技术的微型惯性导航系统的发展现状[J].中国惯性技术学报，2004，12（6）：88?94.

[3] 赵祚喜，罗锡文，李庆，等.基于MEMS惯性传感器融合的水田激光平地机水平控制系统[J].农业工程学报，2008，24（6）：119?124.

[4] 黎永键，赵祚喜.水田激光平地机平地铲姿态测量系统的设计[J].农机化研究，2012，34（2）：69?75.

[5] 侯明亮，毛恩荣，刘刚.激光控制平地系统控制技术的研究与试验[J].农业工程学报，2006，22（1）：110?113.

[6] 胡炼，林潮兴，罗锡文，等.农机具自动调平控制系统设计与试验[J].农业工程学报，2015，31（8）：15?20.

[7] 林建涵.激光控制平地系统接收和控制装置的研究与开发[D].北京：中国农业大学，2004.

[8] 李臣龙，强俊.基于STM32和MPU6050姿态解算的研究与实现[J].佳木斯大学学报，2017，35（2）：295?298.

[9] 潘春荣，许化.基于STM32的X型四旋翼无人机设计[J].工程设计学报，2017，24（2）：196?202.

[10] 黄琦，韩广源，吴瑞东，等.基于STM32的高精度恒温控制系统设计[J].仪表技术与传感器，2017（5）：71?74.

[11] 胡发焕，邱小童，蔡咸健.基于场效应管的大功率直流电机驱动电路设计[J].电机控制与应用技术，2011，38（4）：21?24.

[12] 苑艳华，李四海，南江.基于卡尔曼滤波器的航姿系统测姿算法研究[J].传感技术学报，2011，24（12）：1718?1722.

[13] 罗天资，陈卫兵，邹豪杰，等.直线电机模糊增量PID控制算法的研究[J].测控技术，2011，30（2）：56?59.

[14] 李迎，孙亚飞.基于增量PID的压电微位移器驱动控制系统开发[J].测控技术，2011，30（3）：40?44.

[15] 陈孟元，谢义建，陈跃东.基于四元数改进型互补滤波的MEMS姿态解算[J].電子测量与仪器学报，2015，29（9）：1391?1397.