基于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