基于PID算法的二自由度自調(diào)節(jié)測(cè)高系統(tǒng)設(shè)計(jì)

姜金華，陳永良

(中國(guó)計(jì)量大學(xué)質(zhì)量與安全工程學(xué)院，浙江 杭州 310018)

基于PID算法的二自由度自調(diào)節(jié)測(cè)高系統(tǒng)設(shè)計(jì)

姜金華，陳永良

(中國(guó)計(jì)量大學(xué)質(zhì)量與安全工程學(xué)院，浙江 杭州 310018)

目前，房屋高度測(cè)量大多采用手持激光測(cè)距儀，測(cè)量的垂直度無(wú)法得到保證。針對(duì)高度測(cè)量中存在的垂直度誤差較大、測(cè)量精度和效率均較低的問題，設(shè)計(jì)了一款集調(diào)節(jié)、測(cè)量為一體的自調(diào)節(jié)測(cè)高系統(tǒng)。系統(tǒng)通過三軸加速度計(jì)獲取當(dāng)前云臺(tái)(PTZ)姿態(tài)信息，對(duì)由STM32控制器接收數(shù)據(jù)并分析運(yùn)算輸出的2路脈沖寬度調(diào)制(PWM)信號(hào)進(jìn)行首次調(diào)節(jié)，再運(yùn)用PID控制算法進(jìn)行2次調(diào)節(jié)，最終實(shí)現(xiàn)了二自由度(2-DOF)的自主垂直調(diào)節(jié)。采用激光測(cè)距的方式進(jìn)行高度測(cè)量，并將數(shù)據(jù)上傳主機(jī)進(jìn)行顯示、存儲(chǔ)。通過試驗(yàn)測(cè)試，證明該系統(tǒng)實(shí)現(xiàn)了自調(diào)節(jié)測(cè)高，垂直誤差為±1°，某定點(diǎn)的多次高度測(cè)量誤差為±1 mm，在3種姿態(tài)情況下的垂直調(diào)節(jié)在2 s內(nèi)完成。 由此驗(yàn)證了該設(shè)計(jì)的可行性。該自調(diào)節(jié)測(cè)高系統(tǒng)能夠有效地存儲(chǔ)測(cè)量數(shù)據(jù)，大大提高了測(cè)量的準(zhǔn)確度與效率。

云臺(tái)； STM32； PID； 脈沖寬度調(diào)制； 舵機(jī)； 二自由度； 高度測(cè)量

0 引言

目前，高度測(cè)量在建筑工程中的應(yīng)用非常普遍，其方法有直接測(cè)量、采用多種儀器配合的間接測(cè)量等[1]。近年來(lái)，科學(xué)技術(shù)的發(fā)展帶動(dòng)了工程測(cè)量技術(shù)的發(fā)展，各種新興技術(shù)的誕生與高精密儀器的普及大大提高了高度測(cè)量精度，并減小了測(cè)量誤差[2]。

現(xiàn)在市場(chǎng)上的云臺(tái)(pan tilt zoom,PTZ)多為大型云臺(tái)，采用電機(jī)控制來(lái)調(diào)節(jié)云臺(tái)姿態(tài)[3-4]。但這類云臺(tái)機(jī)械設(shè)計(jì)復(fù)雜、開發(fā)難度較大、需要考慮的問題多，并且成本較高。

本文采用STM32作為主控芯片，結(jié)合與加速度計(jì)、舵機(jī)和激光測(cè)距儀等相配套的機(jī)械結(jié)構(gòu)，研發(fā)了一款自調(diào)節(jié)、測(cè)距一體機(jī)?？紤]到實(shí)用性與經(jīng)濟(jì)性，一體機(jī)的體積應(yīng)盡可能小、成本應(yīng)盡可能低，且各方面指標(biāo)均應(yīng)滿足實(shí)際測(cè)量要求。考慮到測(cè)量精度的要求較高，對(duì)比多種測(cè)距儀器，選擇相位式測(cè)距模塊作為執(zhí)行部件。相位式激光測(cè)距具備測(cè)量穩(wěn)定、精度高且體積小的優(yōu)點(diǎn)，滿足了設(shè)計(jì)中對(duì)測(cè)距部分的各種需求。相較于市面上通過電機(jī)進(jìn)行調(diào)節(jié)的云臺(tái)，本設(shè)計(jì)應(yīng)用舵機(jī)實(shí)現(xiàn)了調(diào)節(jié)控制，在響應(yīng)速度、精度方面均能達(dá)到相同的等級(jí)，并且生產(chǎn)成本更低[5]。

1 系統(tǒng)組成與結(jié)構(gòu)

本設(shè)計(jì)主要由調(diào)節(jié)和測(cè)距2部分組成。調(diào)節(jié)部分又可分為機(jī)械結(jié)構(gòu)和電子電路。調(diào)節(jié)系統(tǒng)結(jié)構(gòu)如圖1所示。

圖1 調(diào)節(jié)系統(tǒng)結(jié)構(gòu)圖

調(diào)節(jié)系統(tǒng)的機(jī)械結(jié)構(gòu)由傳動(dòng)部件、執(zhí)行機(jī)構(gòu)和固定框架構(gòu)成。執(zhí)行機(jī)構(gòu)采用舵機(jī)，其選擇主要考慮偏轉(zhuǎn)角度、扭矩和頻響帶寬[6]。

本設(shè)計(jì)采用XQ-RS420型舵機(jī)，其轉(zhuǎn)速和扭力分別為66.7 r/min(4.8 V)和17.2 kg·cm(4.8 V),死區(qū)寬度為2 μs，以保證調(diào)節(jié)系統(tǒng)具有調(diào)節(jié)精度高、調(diào)節(jié)速度快和負(fù)載能力強(qiáng)的特點(diǎn)。電子電路部分主要包括STM32微處理器控制模塊、電源模塊、三軸加速度計(jì)傳感器模塊以及舵機(jī)控制模塊。

激光測(cè)距分為脈沖式激光測(cè)距和相位式激光測(cè)距。脈沖式激光測(cè)距通過測(cè)算從激光源發(fā)射激光到激光遇到遮擋物返回所需的時(shí)間，以計(jì)算距離[7]。相位式激光測(cè)距通過測(cè)算激光發(fā)射器發(fā)射與反射回的光束之間的相位差計(jì)算距離值[8]。根據(jù)建筑物測(cè)量的需求，本系統(tǒng)選取相位式激光測(cè)距。

二自由度自調(diào)節(jié)測(cè)高系統(tǒng)的基本工作原理為：STM32通過接收固定在載物臺(tái)上的加速度計(jì)在當(dāng)前姿態(tài)下重力加速度在三軸上的垂直分量，計(jì)算云臺(tái)姿態(tài)；與理想狀態(tài)相比，計(jì)算偏差值和需要調(diào)節(jié)的轉(zhuǎn)動(dòng)量，在可調(diào)范圍內(nèi)輸出2路脈沖寬度調(diào)制(pulse width modulation,PWM)信號(hào)，控制2個(gè)舵機(jī)轉(zhuǎn)動(dòng)，以實(shí)現(xiàn)二自由度自調(diào)節(jié)。采用舵機(jī)作為執(zhí)行機(jī)構(gòu)，最小可調(diào)角度為0.18°。相對(duì)于步進(jìn)電機(jī)或直流電機(jī)，采用舵機(jī)作為執(zhí)行機(jī)構(gòu)可以減少煩瑣的軟、硬件設(shè)計(jì),達(dá)到理想效果。

二自由度自調(diào)節(jié)測(cè)高系統(tǒng)結(jié)構(gòu)如圖2所示。

圖2 測(cè)高系統(tǒng)結(jié)構(gòu)圖

2 硬件設(shè)計(jì)

2.1 主控板電路設(shè)計(jì)

本系統(tǒng)采用STM32f103ZET6芯片。該芯片基于ARM Cortex-M3核心的32位微控制器，最高工作頻率為72 MHz。該處理器有8個(gè)16位定時(shí)器，每個(gè)定時(shí)器均有4路輸出通道，可以輸出PWM信號(hào)[9]。該處理器有多達(dá)13個(gè)通信接口。豐富的接口使外設(shè)的選擇更加靈活。主控板電路主要涉及主控芯片與加速度傳感器、舵機(jī)、激光測(cè)距模塊和上位機(jī)之間的通信。由于通信方式的不同，在配置管腳時(shí)，根據(jù)每個(gè)外設(shè)的實(shí)際要求進(jìn)行配置，主要涉及I2C和串口通信。

當(dāng)輸出PWM信號(hào)時(shí)，為防止因外設(shè)連接錯(cuò)誤而毀壞主控芯片，在2路PWM信號(hào)輸出端口處設(shè)計(jì)保護(hù)電路。此處保護(hù)電路的設(shè)計(jì)主要采用LM393芯片。LM393保護(hù)電路需要在LM393比較器與2路PWM輸出端口之間設(shè)計(jì)濾波電路，并在輸出端放置上拉電阻。在保護(hù)I/O口的情況下，對(duì)輸出信號(hào)進(jìn)行濾波去耦，并拉高輸出的PWM高電平信號(hào)，以便輸出穩(wěn)定的PWM信號(hào)，從而增強(qiáng)輸出信號(hào)的強(qiáng)度。

2.2 三軸加速度計(jì)模塊

本系統(tǒng)采用ADXL345三軸加速度計(jì)作為傾斜測(cè)量器件。這款加速度計(jì)的分辨率為13位，能夠檢測(cè)出小于1°的傾斜角變化。該模塊得到當(dāng)前姿態(tài)下傳感器所受重力分別在X、Y、Z軸上的垂直分量后，通過I2C總線將其傳輸?shù)街骺匦酒M(jìn)行處理。重力輸出響應(yīng)如圖3所示。

圖3 重力輸出響應(yīng)示意圖

3 軟件設(shè)計(jì)

3.1 系統(tǒng)整體程序

系統(tǒng)整體設(shè)計(jì)中涉及多種硬件設(shè)備。通過對(duì)三軸加速度計(jì)所獲取的數(shù)據(jù)進(jìn)行處理，控制舵機(jī)測(cè)距模塊工作。整體程序流程如圖4所示。

圖4 整體程序流程圖

3.2 姿態(tài)確定程序

三軸加速度計(jì)檢測(cè)三軸分量。當(dāng)發(fā)生傾斜后，重力G的方向偏轉(zhuǎn)，獲取G在Z軸映射的分量Gz；在XOY平面垂直映射G’，將G’在XOY平面正交分解，分別獲取G’在X軸的分量Gx和在Y軸的分量Gy。判斷G’在XOY平面所在的象限，可知傳感器的傾斜狀態(tài)。將三軸分量轉(zhuǎn)換成偏轉(zhuǎn)角度，可以確定芯片的姿態(tài)。通過式(1)～式(3)，可進(jìn)行角度轉(zhuǎn)化。

(1)

(2)

(3)

在姿態(tài)確定過程中，會(huì)存在誤差。誤差主要來(lái)自安裝過程中造成的系統(tǒng)誤差、舵機(jī)調(diào)節(jié)導(dǎo)致的載物臺(tái)的抖動(dòng)和加速度計(jì)自身的非線性偏差[10]。在安裝過程中，應(yīng)盡量減小系統(tǒng)誤差。對(duì)此，可通過在程序中添加延時(shí)來(lái)減小抖動(dòng)造成的誤差，并通過均值濾波來(lái)減小自身存在的非線性誤差。

3.3 舵機(jī)控制程序

自調(diào)節(jié)系統(tǒng)的調(diào)節(jié)部分由2個(gè)舵機(jī)控制。當(dāng)載物臺(tái)發(fā)生傾斜時(shí)，主控芯片分析姿態(tài)信息并計(jì)算輸出2路PWM信號(hào)，以分別控制舵機(jī)轉(zhuǎn)動(dòng)。舵機(jī)在控制時(shí)需要20 ms的時(shí)基脈沖，高電平在0.5～2.5 ms之間。程序輸出2路占空比為7.5%PWM信號(hào)復(fù)位舵機(jī)位置[11-12]。復(fù)位后，STM32讀取加速度計(jì)在三軸上的垂直分量Gx、Gy、Gz。假設(shè)檢測(cè)到載物臺(tái)傾斜，Gx>0、Gy>0、Gz>0，則重力投影在XOY平面上的第一象限。重力投影圖如圖5所示。

圖5 重力投影圖

根據(jù)圖5，載物臺(tái)需要繞Z軸轉(zhuǎn)動(dòng)的角度為α。

α=atan2(Gx,Gy)

(4)

atan2函數(shù)返回的方位角，比atan函數(shù)更加穩(wěn)定。應(yīng)用atan函數(shù)時(shí)，如Gy遠(yuǎn)大于Gx，會(huì)因返回值過大而導(dǎo)致輸出不穩(wěn)定的情況；而使用atan2返回方位角則可避免此類問題的發(fā)生。根據(jù)式(5)，可得STM32輸出比較寄存器值為：

(5)

(6)

Kp、Kd系數(shù)需要根據(jù)實(shí)際情況進(jìn)行調(diào)試。調(diào)整后，再次讀取姿態(tài)信息。若θx不在±1°范圍內(nèi)，則再次調(diào)用比例微分控制算法調(diào)整姿態(tài)，直到θx達(dá)到該范圍內(nèi)。

舵機(jī)A調(diào)整后，載物臺(tái)重力落在YOZ平面。STM32讀取當(dāng)前姿態(tài)信息，計(jì)算出重力G在YOZ平面與Z軸的偏差角度θy；根據(jù)式(7)，計(jì)算出舵機(jī)B首次調(diào)整所需要的輸出比較寄存器CCR2數(shù)值。

(7)

舵機(jī)B首次調(diào)整后，由STM32讀取當(dāng)前姿態(tài)信息。若θy不在±1°范圍內(nèi)，則調(diào)用比例微分控制算法進(jìn)行調(diào)整，直到θy達(dá)到該范圍內(nèi)。

云臺(tái)發(fā)生傾斜后，經(jīng)多次調(diào)整后的效果如圖6所示。

圖6 云臺(tái)調(diào)整效果圖

3.4 測(cè)距與顯示

采用相位式激光測(cè)距模塊，通過串口對(duì)模塊發(fā)送的指令進(jìn)行模塊驅(qū)動(dòng)控制。當(dāng)按下主控板上的按鈕后,程序進(jìn)入中斷，通過USART1對(duì)測(cè)距模塊發(fā)送打開指令； 當(dāng)STM32接收到回饋信息后,再發(fā)送測(cè)量指令; 主控芯片將接收到的測(cè)量數(shù)據(jù)通過USART2傳輸?shù)接?jì)算機(jī)，并對(duì)數(shù)據(jù)進(jìn)行分析、處理后保存。

4 試驗(yàn)驗(yàn)證

實(shí)際測(cè)試中，當(dāng)云臺(tái)發(fā)生傾斜時(shí)，本文設(shè)計(jì)的系統(tǒng)能夠確定當(dāng)前姿態(tài)并使測(cè)距裝置保持垂直狀態(tài)。整個(gè)調(diào)整過程時(shí)間短、垂直度高，證明了該系統(tǒng)能提高高度測(cè)量的準(zhǔn)確度。調(diào)試過程中，選取了3種特殊的傾斜狀態(tài)。試驗(yàn)結(jié)果如表1所示。

表1 試驗(yàn)結(jié)果

對(duì)表1中的數(shù)據(jù)進(jìn)行分析，在測(cè)試條件相同的情況下，測(cè)試各種傾斜狀態(tài)下姿態(tài)調(diào)整的過程。測(cè)試結(jié)果顯示，在各種狀態(tài)下，姿態(tài)調(diào)整的相對(duì)誤差與調(diào)整時(shí)間各不相同。由于一次調(diào)整存在偏差，引入PID閉環(huán)控制進(jìn)行修正。最終，整體調(diào)整時(shí)間有所增加，但仍保持在2 s之內(nèi)，調(diào)整偏差角度在±1°內(nèi)。

在高度測(cè)量中，測(cè)量值的顯示部分采用LabVIEW編寫界面。通過對(duì)一個(gè)點(diǎn)進(jìn)行多次測(cè)量以減少誤差，并得到更精確的高度值。主控芯片接收測(cè)距模塊測(cè)得的多組數(shù)據(jù)并上傳至上位機(jī)；上位機(jī)接收測(cè)量的數(shù)據(jù)，分析并拆分?jǐn)?shù)據(jù)，同時(shí)對(duì)每個(gè)數(shù)據(jù)添加時(shí)間和地點(diǎn)標(biāo)簽，以實(shí)現(xiàn)對(duì)數(shù)據(jù)的分類顯示與存儲(chǔ)。上位機(jī)界面主要放置串口參數(shù)、文件存儲(chǔ)地址選項(xiàng)和分類處理后對(duì)每組數(shù)據(jù)的顯示控件，最終實(shí)現(xiàn)界面對(duì)上傳數(shù)據(jù)的自定義保存與分類顯示。

5 結(jié)束語(yǔ)

傳統(tǒng)的測(cè)量手段無(wú)法保證測(cè)量?jī)x器是否與地面保持垂直。本設(shè)計(jì)通過對(duì)云臺(tái)姿態(tài)進(jìn)行控制調(diào)整，當(dāng)裝置在發(fā)生傾斜時(shí)，能夠?qū)崿F(xiàn)自我調(diào)整，使載物臺(tái)恢復(fù)水平狀態(tài)，從而確保固定在載物臺(tái)上的激光測(cè)距模塊始終與地面保持垂直狀態(tài)。

在應(yīng)用到傳統(tǒng)的樓房測(cè)高時(shí)，本設(shè)計(jì)比傳統(tǒng)的卷尺或手持式激光測(cè)距儀更方便、準(zhǔn)確。引入PID算法控制舵機(jī)調(diào)整姿態(tài)后，電機(jī)調(diào)整更快速、簡(jiǎn)便，大大降低了成本、提高了效率。

[1] 李玉濤.工程測(cè)量在實(shí)際施工的重要作用[J].科技創(chuàng)業(yè)家,2013,4(11):21.

[2] 鄧紹云.工程測(cè)量發(fā)展現(xiàn)狀與趨勢(shì)分析[J].科技視界,2015,5(8):13.

[3] 李明海,徐軼群.基于單片機(jī)的云臺(tái)監(jiān)控控制器的設(shè)計(jì)[J].機(jī)電工程技術(shù),2014,38(9):104-107.

[4] 富曉杰.高精度伺服云臺(tái)的機(jī)械結(jié)構(gòu)與控制系統(tǒng)研究[D].杭州：浙江大學(xué),2014.

[5] 鄭思航,陳永良,李琛,等.基于陀螺儀與伺服舵機(jī)的自動(dòng)安平系統(tǒng)設(shè)計(jì)[J].機(jī)電工程,2013,20(12):1509-1514.

[6] 張文文,王建偉,侯文,等.電動(dòng)舵機(jī)設(shè)計(jì)中直流電機(jī)選型新方法[J].自動(dòng)化儀表,2016,37(9):95-98.

[7] 胥俊丞.新型相位激光測(cè)距儀的研究[D].西安：西安電子科技大學(xué),2008.

[8] 吳應(yīng)明.便攜式脈沖激光測(cè)距儀的研制[D].西安：西安電子科技大學(xué),2009.

[9] 孫書鷹,陳志佳,寇超.新一代嵌入式微處理器STM32F103開發(fā)與應(yīng)用[J].微計(jì)算機(jī)應(yīng)用,2010,31(12):59-63.

[10]陳殿生,邵志浩,雷旭升,等.MEMS陀螺儀隨機(jī)誤差濾波[J].

北京航空航天大學(xué)學(xué)報(bào),2009,54(2):246-250.

[11]周永龍,雷金奎.基于STM32的數(shù)字舵機(jī)控制系統(tǒng)的設(shè)計(jì)[J].計(jì)算機(jī)測(cè)量與控制,2011,19(1):66-68.

[12]韓玉龍,趙瑾,申忠宇,等.競(jìng)賽機(jī)器人多舵機(jī)控制方法的研究與實(shí)現(xiàn)[J].自動(dòng)化儀表,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—)，男，在讀碩士研究生，主要從事測(cè)量與微機(jī)自動(dòng)化控制方向的研究，E-mail：s1506081103@stu.cjlu.edu.cn；陳永良(通信作者)，男，碩士，教授，主要從事測(cè)量與微機(jī)自動(dòng)化控制方向的研究，E-mail：cimcyl@cjlu.edu.cn

TH711；TP272

A

10.16086/j.cnki.issn1000-0380.201712003