基于PID算法的二自由度自调节测高系统设计

姜金华，陈永良

(中国计量大学质量与安全工程学院，浙江 杭州 310018)

基于PID算法的二自由度自调节测高系统设计

姜金华，陈永良

(中国计量大学质量与安全工程学院，浙江 杭州 310018)

目前，房屋高度测量大多采用手持激光测距仪，测量的垂直度无法得到保证。针对高度测量中存在的垂直度误差较大、测量精度和效率均较低的问题，设计了一款集调节、测量为一体的自调节测高系统。系统通过三轴加速度计获取当前云台(PTZ)姿态信息，对由STM32控制器接收数据并分析运算输出的2路脉冲宽度调制(PWM)信号进行首次调节，再运用PID控制算法进行2次调节，最终实现了二自由度(2-DOF)的自主垂直调节。采用激光测距的方式进行高度测量，并将数据上传主机进行显示、存储。通过试验测试，证明该系统实现了自调节测高，垂直误差为±1°，某定点的多次高度测量误差为±1 mm，在3种姿态情况下的垂直调节在2 s内完成。 由此验证了该设计的可行性。该自调节测高系统能够有效地存储测量数据，大大提高了测量的准确度与效率。

云台； STM32； PID； 脉冲宽度调制； 舵机； 二自由度； 高度测量

0 引言

目前，高度测量在建筑工程中的应用非常普遍，其方法有直接测量、采用多种仪器配合的间接测量等[1]。近年来，科学技术的发展带动了工程测量技术的发展，各种新兴技术的诞生与高精密仪器的普及大大提高了高度测量精度，并减小了测量误差[2]。

现在市场上的云台(pan tilt zoom,PTZ)多为大型云台，采用电机控制来调节云台姿态[3-4]。但这类云台机械设计复杂、开发难度较大、需要考虑的问题多，并且成本较高。

本文采用STM32作为主控芯片，结合与加速度计、舵机和激光测距仪等相配套的机械结构，研发了一款自调节、测距一体机。考虑到实用性与经济性，一体机的体积应尽可能小、成本应尽可能低，且各方面指标均应满足实际测量要求。考虑到测量精度的要求较高，对比多种测距仪器，选择相位式测距模块作为执行部件。相位式激光测距具备测量稳定、精度高且体积小的优点，满足了设计中对测距部分的各种需求。相较于市面上通过电机进行调节的云台，本设计应用舵机实现了调节控制，在响应速度、精度方面均能达到相同的等级，并且生产成本更低[5]。

1 系统组成与结构

本设计主要由调节和测距2部分组成。调节部分又可分为机械结构和电子电路。调节系统结构如图1所示。

图1 调节系统结构图

调节系统的机械结构由传动部件、执行机构和固定框架构成。执行机构采用舵机，其选择主要考虑偏转角度、扭矩和频响带宽[6]。

本设计采用XQ-RS420型舵机，其转速和扭力分别为66.7 r/min(4.8 V)和17.2 kg·cm(4.8 V),死区宽度为2 μs，以保证调节系统具有调节精度高、调节速度快和负载能力强的特点。电子电路部分主要包括STM32微处理器控制模块、电源模块、三轴加速度计传感器模块以及舵机控制模块。

激光测距分为脉冲式激光测距和相位式激光测距。脉冲式激光测距通过测算从激光源发射激光到激光遇到遮挡物返回所需的时间，以计算距离[7]。相位式激光测距通过测算激光发射器发射与反射回的光束之间的相位差计算距离值[8]。根据建筑物测量的需求，本系统选取相位式激光测距。

二自由度自调节测高系统的基本工作原理为：STM32通过接收固定在载物台上的加速度计在当前姿态下重力加速度在三轴上的垂直分量，计算云台姿态；与理想状态相比，计算偏差值和需要调节的转动量，在可调范围内输出2路脉冲宽度调制(pulse width modulation,PWM)信号，控制2个舵机转动，以实现二自由度自调节。采用舵机作为执行机构，最小可调角度为0.18°。相对于步进电机或直流电机，采用舵机作为执行机构可以减少烦琐的软、硬件设计,达到理想效果。

二自由度自调节测高系统结构如图2所示。

图2 测高系统结构图

2 硬件设计

2.1 主控板电路设计

本系统采用STM32f103ZET6芯片。该芯片基于ARM Cortex-M3核心的32位微控制器，最高工作频率为72 MHz。该处理器有8个16位定时器，每个定时器均有4路输出通道，可以输出PWM信号[9]。该处理器有多达13个通信接口。丰富的接口使外设的选择更加灵活。主控板电路主要涉及主控芯片与加速度传感器、舵机、激光测距模块和上位机之间的通信。由于通信方式的不同，在配置管脚时，根据每个外设的实际要求进行配置，主要涉及I2C和串口通信。

当输出PWM信号时，为防止因外设连接错误而毁坏主控芯片，在2路PWM信号输出端口处设计保护电路。此处保护电路的设计主要采用LM393芯片。LM393保护电路需要在LM393比较器与2路PWM输出端口之间设计滤波电路，并在输出端放置上拉电阻。在保护I/O口的情况下，对输出信号进行滤波去耦，并拉高输出的PWM高电平信号，以便输出稳定的PWM信号，从而增强输出信号的强度。

2.2 三轴加速度计模块

本系统采用ADXL345三轴加速度计作为倾斜测量器件。这款加速度计的分辨率为13位，能够检测出小于1°的倾斜角变化。该模块得到当前姿态下传感器所受重力分别在X、Y、Z轴上的垂直分量后，通过I2C总线将其传输到主控芯片进行处理。重力输出响应如图3所示。

图3 重力输出响应示意图

3 软件设计

3.1 系统整体程序

系统整体设计中涉及多种硬件设备。通过对三轴加速度计所获取的数据进行处理，控制舵机测距模块工作。整体程序流程如图4所示。

图4 整体程序流程图

3.2 姿态确定程序

三轴加速度计检测三轴分量。当发生倾斜后，重力G的方向偏转，获取G在Z轴映射的分量Gz；在XOY平面垂直映射G’，将G’在XOY平面正交分解，分别获取G’在X轴的分量Gx和在Y轴的分量Gy。判断G’在XOY平面所在的象限，可知传感器的倾斜状态。将三轴分量转换成偏转角度，可以确定芯片的姿态。通过式(1)～式(3)，可进行角度转化。

(1)

(2)

(3)

在姿态确定过程中，会存在误差。误差主要来自安装过程中造成的系统误差、舵机调节导致的载物台的抖动和加速度计自身的非线性偏差[10]。在安装过程中，应尽量减小系统误差。对此，可通过在程序中添加延时来减小抖动造成的误差，并通过均值滤波来减小自身存在的非线性误差。

3.3 舵机控制程序

自调节系统的调节部分由2个舵机控制。当载物台发生倾斜时，主控芯片分析姿态信息并计算输出2路PWM信号，以分别控制舵机转动。舵机在控制时需要20 ms的时基脉冲，高电平在0.5～2.5 ms之间。程序输出2路占空比为7.5%PWM信号复位舵机位置[11-12]。复位后，STM32读取加速度计在三轴上的垂直分量Gx、Gy、Gz。假设检测到载物台倾斜，Gx>0、Gy>0、Gz>0，则重力投影在XOY平面上的第一象限。重力投影图如图5所示。

图5 重力投影图

根据图5，载物台需要绕Z轴转动的角度为α。

α=atan2(Gx,Gy)

(4)

atan2函数返回的方位角，比atan函数更加稳定。应用atan函数时，如Gy远大于Gx，会因返回值过大而导致输出不稳定的情况；而使用atan2返回方位角则可避免此类问题的发生。根据式(5)，可得STM32输出比较寄存器值为：

(5)

(6)

Kp、Kd系数需要根据实际情况进行调试。调整后，再次读取姿态信息。若θx不在±1°范围内，则再次调用比例微分控制算法调整姿态，直到θx达到该范围内。

舵机A调整后，载物台重力落在YOZ平面。STM32读取当前姿态信息，计算出重力G在YOZ平面与Z轴的偏差角度θy；根据式(7)，计算出舵机B首次调整所需要的输出比较寄存器CCR2数值。

(7)

舵机B首次调整后，由STM32读取当前姿态信息。若θy不在±1°范围内，则调用比例微分控制算法进行调整，直到θy达到该范围内。

云台发生倾斜后，经多次调整后的效果如图6所示。

图6 云台调整效果图

3.4 测距与显示

采用相位式激光测距模块，通过串口对模块发送的指令进行模块驱动控制。当按下主控板上的按钮后,程序进入中断，通过USART1对测距模块发送打开指令； 当STM32接收到回馈信息后,再发送测量指令; 主控芯片将接收到的测量数据通过USART2传输到计算机，并对数据进行分析、处理后保存。

4 试验验证

实际测试中，当云台发生倾斜时，本文设计的系统能够确定当前姿态并使测距装置保持垂直状态。整个调整过程时间短、垂直度高，证明了该系统能提高高度测量的准确度。调试过程中，选取了3种特殊的倾斜状态。试验结果如表1所示。

表1 试验结果

对表1中的数据进行分析，在测试条件相同的情况下，测试各种倾斜状态下姿态调整的过程。测试结果显示，在各种状态下，姿态调整的相对误差与调整时间各不相同。由于一次调整存在偏差，引入PID闭环控制进行修正。最终，整体调整时间有所增加，但仍保持在2 s之内，调整偏差角度在±1°内。

在高度测量中，测量值的显示部分采用LabVIEW编写界面。通过对一个点进行多次测量以减少误差，并得到更精确的高度值。主控芯片接收测距模块测得的多组数据并上传至上位机；上位机接收测量的数据，分析并拆分数据，同时对每个数据添加时间和地点标签，以实现对数据的分类显示与存储。上位机界面主要放置串口参数、文件存储地址选项和分类处理后对每组数据的显示控件，最终实现界面对上传数据的自定义保存与分类显示。

5 结束语

传统的测量手段无法保证测量仪器是否与地面保持垂直。本设计通过对云台姿态进行控制调整，当装置在发生倾斜时，能够实现自我调整，使载物台恢复水平状态，从而确保固定在载物台上的激光测距模块始终与地面保持垂直状态。

在应用到传统的楼房测高时，本设计比传统的卷尺或手持式激光测距仪更方便、准确。引入PID算法控制舵机调整姿态后，电机调整更快速、简便，大大降低了成本、提高了效率。

[1] 李玉涛.工程测量在实际施工的重要作用[J].科技创业家,2013,4(11):21.

[2] 邓绍云.工程测量发展现状与趋势分析[J].科技视界,2015,5(8):13.

[3] 李明海,徐轶群.基于单片机的云台监控控制器的设计[J].机电工程技术,2014,38(9):104-107.

[4] 富晓杰.高精度伺服云台的机械结构与控制系统研究[D].杭州：浙江大学,2014.

[5] 郑思航,陈永良,李琛,等.基于陀螺仪与伺服舵机的自动安平系统设计[J].机电工程,2013,20(12):1509-1514.

[6] 张文文,王建伟,侯文,等.电动舵机设计中直流电机选型新方法[J].自动化仪表,2016,37(9):95-98.

[7] 胥俊丞.新型相位激光测距仪的研究[D].西安：西安电子科技大学,2008.

[8] 吴应明.便携式脉冲激光测距仪的研制[D].西安：西安电子科技大学,2009.

[9] 孙书鹰,陈志佳,寇超.新一代嵌入式微处理器STM32F103开发与应用[J].微计算机应用,2010,31(12):59-63.

[10]陈殿生,邵志浩,雷旭升,等.MEMS陀螺仪随机误差滤波[J].

北京航空航天大学学报,2009,54(2):246-250.

[11]周永龙,雷金奎.基于STM32的数字舵机控制系统的设计[J].计算机测量与控制,2011,19(1):66-68.

[12]韩玉龙,赵瑾,申忠宇,等.竞赛机器人多舵机控制方法的研究与实现[J].自动化仪表,2016,37(7):46-49.

Designofthe2-DOFSelf-AdjustingHeightMeasurementSystemBasedonPIDAlgorithm

JIANG Jinhua,CHEN Yongliang

(College of Quality and Safety Engineering,China Jiliang University,Hangzhou 310018,China)

Presently,most of the building height measurement uses hand-held laser range finder,and the measurement verticality cannot be guaranteed.To solve the problems existing in height measurement,e.g.,large error of verticality,low accuracy of measurement and efficiency,a self-adjusting height measurement system which integrates adjustment and measurement has been designed.In the system,the current pan tilt zoom(PTZ) attitude information is obtained through the three-axis accelerometer,then the data are

,analyzed and calculated by STM32,and two of pulse width modulation(PWM) signals are output for the first adjustment; the second adjustment is carried out by using PID control algorithm,to realize the autonomous two degrees of freedom(2-DOF) vertical adjustment finally.The height measurement is realized by using laser ranging method,and the data are uploaded to the host computer for display and storage.Through the experimental tests,the self-adjusting altimetry is implemented.The vertical error is ±1° and height measuring error at fixed point repeatedly is ±1 mm.Under the condition of 3 kinds of gestures,the vertical adjustment is completed in 2 s,which verifies the feasibility of the design.The self-adjusting height measurement system can effectively store measurement data,greatly improve the measurement accuracy and efficiency.

Pan tilt zoom(PTZ)； STM32； PID； Pulse width modulation(PWM)； Steering engine； Two degrees of freedom(2-DOF)； Height measurement

修改稿收到日期:2017-03-27

姜金华(1992—)，男，在读硕士研究生，主要从事测量与微机自动化控制方向的研究，E-mail：s1506081103@stu.cjlu.edu.cn；陈永良(通信作者)，男，硕士，教授，主要从事测量与微机自动化控制方向的研究，E-mail：cimcyl@cjlu.edu.cn

TH711；TP272

A

10.16086/j.cnki.issn1000-0380.201712003