林思翔

（福建農(nóng)林大學(xué) 電氣工程系，福建 福州 350002）

基于單片機(jī)的風(fēng)力擺系統(tǒng)設(shè)計(jì)

林思翔

（福建農(nóng)林大學(xué) 電氣工程系，福建 福州 350002）

介紹一種以單片機(jī)為核心的風(fēng)力擺姿勢控制系統(tǒng)。該系統(tǒng)由瑞薩R5F100LEA 16位MCU為控制核心，采用Kalman濾波融合GY-85九軸自由度IMU傳感器實(shí)時采集風(fēng)力擺的位姿，通過單片機(jī)接收數(shù)據(jù)，經(jīng)PID校正，調(diào)整PWM的占空比來控制風(fēng)機(jī)的轉(zhuǎn)速及轉(zhuǎn)向，從而實(shí)現(xiàn)對風(fēng)力擺的控制。測試結(jié)果表明，基于單片機(jī)的風(fēng)力擺系統(tǒng)的設(shè)計(jì)，可以較好的實(shí)現(xiàn)風(fēng)力擺的快速起擺、走直線、走圓形軌跡、制動靜止、走數(shù)字8形等功能。

R5F100LEA；傳感器；PID；Kalman濾波

隨著現(xiàn)代測控技術(shù)的迅猛發(fā)展，以微處理器為基礎(chǔ)，在交通、醫(yī)學(xué)、化工、機(jī)器人、國防等領(lǐng)域廣泛應(yīng)用。文章提出一種基于單片機(jī)的風(fēng)力擺控制系統(tǒng)，通過GY-85九軸自由度IMU，對擺桿的運(yùn)行狀態(tài)數(shù)據(jù)進(jìn)行實(shí)時采集并反饋回單片機(jī)，形成閉環(huán)控制系統(tǒng)。單片機(jī)對返回的數(shù)據(jù)結(jié)合基于卡爾曼濾波的PID控制算法進(jìn)行處理，產(chǎn)生的控制量作用于驅(qū)動電路,從而實(shí)現(xiàn)對四個軸流風(fēng)機(jī)轉(zhuǎn)速及轉(zhuǎn)向的控制。四個軸流風(fēng)機(jī)產(chǎn)生空氣推動各風(fēng)機(jī)使風(fēng)力擺按照一定規(guī)律運(yùn)動。

1 系統(tǒng)構(gòu)建

本系統(tǒng)的機(jī)械結(jié)構(gòu)是將萬向節(jié)與自由擺桿鑲嵌后，固定于木架上。風(fēng)力擺豎直向下，在擺桿的末端固定主控板、激光頭和軸流風(fēng)機(jī)。其中，軸流風(fēng)機(jī)采用四旋翼飛行器所用的空心杯電機(jī)及其螺旋槳，布局采用十字形布局，朝向分別為上下左右。整個系統(tǒng)由MCU核心模塊、陀螺儀測量反饋、電機(jī)驅(qū)動模塊、按鍵和LED顯示模塊及電池組成，系統(tǒng)總體框圖如圖1所示。

圖1 系統(tǒng)總體框圖

2 系統(tǒng)硬件電路

系統(tǒng)采用瑞薩R5F100LEA作主控芯片，基于16位CISC架構(gòu)，內(nèi)置高速片上振蕩器時鐘及按鍵中斷，具有超強(qiáng)的模擬量處理功能，支持On-chip調(diào)試功能。采用核心板和底板兩級接插方式，方便進(jìn)行調(diào)節(jié)與功能轉(zhuǎn)換。底板包括JTAG電路與相應(yīng)的濾波穩(wěn)壓電路，如圖2所示。

圖2 微控制系統(tǒng)核心電路圖

GY-85作為九軸自由度IMU傳感器，集成三軸陀螺儀ITG3205、三軸加速度傳感器ADXL345、三軸磁場強(qiáng)度傳感器HMC5883L，可選擇的測量范圍有±2g，±4g，±8 g或±16 g，具有自動調(diào)節(jié)功耗的功能。本系統(tǒng)通過SPI接口以中斷方式快速讀取數(shù)據(jù)。為提高測量精度，在電源處采用多個電容進(jìn)行去耦，從而實(shí)時反應(yīng)目標(biāo)多方位的運(yùn)行狀態(tài)，并進(jìn)行數(shù)據(jù)的還原和校正，如圖3所示。

圖3 GY-85芯片部分電路圖

電機(jī)驅(qū)動采用MOS管SI2300輸出方式提高帶載能力，反接的二極管IN41478和0.1uf電容起到保護(hù)作用，電阻R3用于防止過電壓擊穿MOS管。電機(jī)的轉(zhuǎn)速通過PWM調(diào)節(jié)占空比來調(diào)節(jié)，如圖4所示。

圖4 電機(jī)驅(qū)動模塊

TPS62203DBVR作為高效率同步降壓轉(zhuǎn)換器，響應(yīng)速度快，靜態(tài)功耗小，如圖5所示。

圖5 加速度計(jì)——陀螺儀模塊供電電路

如圖6所示為功能按鍵和LED顯示的簡易電路，按鍵和顯示模塊使風(fēng)力擺各個功能穩(wěn)定有序的實(shí)現(xiàn)和提示。

圖6 LED顯示電路

3 系統(tǒng)軟件設(shè)計(jì)

系統(tǒng)軟件設(shè)計(jì)主要由兩部分組成。主程序主要負(fù)責(zé)總體程序管理功能，實(shí)現(xiàn)人機(jī)交互設(shè)定。在單片機(jī)開機(jī)延時初始化后，啟動主程序，通過按鍵掃描選擇相應(yīng)功能，并顯示指示燈，具體流程圖如圖7所示。

圖7 主程序流程圖

中斷服務(wù)子程序流程圖如圖8所示，初始化子程序的主要工作是設(shè)置定時器的工作模式，初值設(shè)定，開中斷等。本程序工作于16位定時模式，定時器初始值裝入后，則經(jīng)陀螺儀采集數(shù)據(jù)反饋數(shù)據(jù)，運(yùn)用PID控制算法融合卡爾曼濾波，利用前一時刻的估計(jì)值和當(dāng)前時刻的觀測值來更新對狀態(tài)變量的估計(jì)，修正輸出PWM后退出中斷子程序。

圖8 中斷服務(wù)子程序流程圖

風(fēng)力擺的控制采用PID閉環(huán)控制方式結(jié)構(gòu)，可寫為等式：

本系統(tǒng)采用手動整定的方法設(shè)置各參數(shù)。其中，Kp是比例增益，TI為積分時間參數(shù)，TD為微分時間參數(shù)，先通過調(diào)整基本項(xiàng)積分分量，加快系統(tǒng)對偏差的響應(yīng)，阻礙輸出效果值偏離目標(biāo)值，再逐步加大比例系數(shù)，壓制擺動消除偏差，阻礙效果值曲線傾斜，若出現(xiàn)抖動，減小比例系數(shù)或引入微分部分阻礙效果值曲線彎曲，進(jìn)而改善了系統(tǒng)的穩(wěn)定性能。由于上述等式僅可用在模擬系統(tǒng)中，為了滿足數(shù)字設(shè)備的使用需求，各分量應(yīng)先被離散化形成增量式PID公式：

其中，KI=Kp·T/TI為積分系數(shù)，KD=Kp·TD/T為微分系數(shù)。使用PID控制器，控制執(zhí)行機(jī)構(gòu)實(shí)時跟蹤系統(tǒng)輸出，使風(fēng)力擺滿足各項(xiàng)要求，如圖9所示。

圖9 PID調(diào)節(jié)

由于加速度計(jì)不能區(qū)分重力加速度和運(yùn)動加速度，需用陀螺儀測量角速率，并利用最小二乘法對其進(jìn)行補(bǔ)償以減少漂移，進(jìn)而實(shí)現(xiàn)較為精確的測量。將陀螺儀的坐標(biāo)進(jìn)行換算以投影到加速度計(jì)的坐標(biāo)平面上去。

Gyro［0］=Symbol_x/14.375；

Gyro［1］=Symbol_y/14.375；

Gyro［2］=Symbol_z/14.375；

RwAcc［0］=A_Symbol_x/256；

RwAcc［1］=A_Symbol_y/256；

RwAcc［2］=A_Symbol_z/256；

RwEst［0］=（RwAcc［0］+wGyro+RwGyro［0］）/（1+wGyro）；

RwEst［1］=（RwAcc［1］+wGyro+RwGyro［1］）/（1+wGyro）；

RwEst［2］=（RwAcc［2］+wGyro+RwGyro［2］）/（1+wGyro）；

將陀螺儀數(shù)值除以14.375轉(zhuǎn)換成為以°/秒為單位的數(shù)值，再將加速度傳感器的數(shù)值除以256轉(zhuǎn)換算成以重力加速度為單位后，二者加權(quán)平均，得出初略位姿角度。為了得到更精確的數(shù)據(jù)，通過藍(lán)牙將三軸位姿初略值傳出發(fā)現(xiàn)，在靜止情況下，三軸位姿圍繞均線上下波動，為此需要使用濾波技術(shù)增強(qiáng)抗干擾能力。采用Kalman濾波方法來改善，如圖10所示。

圖10 三軸位姿波動值

圖11 風(fēng)力擺受力圖

為使風(fēng)力擺可任角度起擺并畫出不短于20cm的直線段則需四臺軸流風(fēng)機(jī)相互配合，以形成合力方可實(shí)現(xiàn)。因四臺風(fēng)機(jī)分別位于X、Y軸線上，所以只需要兩個軸線上的風(fēng)機(jī)各自產(chǎn)生的空氣推力呈比例，即tanθ=Fy/Fx，如圖11所示；再通過MCU控制電機(jī)的PWM，反復(fù)調(diào)節(jié)，即可使擺動角度達(dá)到要求。

要實(shí)現(xiàn)激光筆繪制的軌跡能在30～60cm間可設(shè)置走直線，令擺桿沿X軸向擺動。為排除可能出現(xiàn)的擾動，將Y軸向平衡角度設(shè)置為0；設(shè)任意直線段長度30～60cm為△R，則對應(yīng)擺桿的角度閾值為Δθ=arctan（ΔR/L1+L2）：。由此可得，X軸向的兩個閾值平衡點(diǎn)。

Δθ1=arctan（ΔR/L1+L2））

Δθ2=-arctan（ΔR/L1+L2））

其中，R為橫向擺動半長，L1為擺桿長，L2為激光頭長；當(dāng)傳感器X軸返回的傾角數(shù)據(jù)大于等于設(shè)定的Δθ1，切換PID算法，向X軸反方向運(yùn)動，當(dāng)X軸返回的傾角數(shù)據(jù)小于等于設(shè)定的Δθ2，切換PID算法，往X軸正方向運(yùn)動，如此往復(fù)運(yùn)行。

將風(fēng)力擺拉起一定角度（30°：45°）放開，令其5s內(nèi)使風(fēng)力擺制動靜止，則需對現(xiàn)場采集的數(shù)據(jù)進(jìn)行實(shí)時的更新和處理，并采用類似于自平衡的PID控制算法即可。以風(fēng)力擺靜止時激光筆的光點(diǎn)為圓心，驅(qū)動風(fēng)力擺用激光筆在地面畫圓，30s內(nèi)重復(fù)3次；圓半徑在15～35cm范圍內(nèi)設(shè)置，使激光筆畫出的軌跡落在指定半徑±2.5cm的圓環(huán)內(nèi)。四個軸流風(fēng)機(jī)A、B、C、D，徑向風(fēng)機(jī)控制采用PID控制算法，使擺桿穩(wěn)定在某一設(shè)定角度，切線方向控制B、D電機(jī)產(chǎn)生推力，推動擺桿圓周運(yùn)動，如圖12所示。

圖12 風(fēng)力擺圓周運(yùn)動示意圖

驅(qū)動風(fēng)力擺實(shí)現(xiàn)走數(shù)字8的功能，只需令風(fēng)力擺以某角度快速起擺后，快速改變電機(jī)、轉(zhuǎn)速、轉(zhuǎn)向，驅(qū)動風(fēng)力擺擺過一定圓弧；再反向直線擺動并走過圓弧，如此反復(fù)運(yùn)行。

4 結(jié)語

系統(tǒng)采用PID數(shù)字控制算法，由卡爾曼濾波對控制信號進(jìn)行濾波處理，修正GY-85九軸自由度IMU傳感器輸出量的隨機(jī)分量，從而減小參數(shù)變化和非線性因素對系統(tǒng)性能的影響，表現(xiàn)出良好的運(yùn)動軌跡，達(dá)到抑制超調(diào)，減小系統(tǒng)穩(wěn)態(tài)誤差，加快系統(tǒng)的瞬態(tài)響應(yīng)，改善系統(tǒng)的穩(wěn)定性能的效果。本系統(tǒng)實(shí)現(xiàn)風(fēng)力擺任角度快速起擺、走直線、制動靜止、走圓形軌跡及走數(shù)字8的功能。

［1］譚浩強(qiáng).C語言程序設(shè)計(jì)［M］．北京：清華大學(xué)出版社，2012．

［2］趙曉安.MCS-51單片機(jī)原理及應(yīng)用［M］.天津：天津大學(xué)出版社，2001.

［3］張毅剛.MCS-51單片應(yīng)用設(shè)計(jì)（第3版）［M］.北京：哈爾濱工業(yè)大學(xué)出版社，2008.

［4］魏惠軍，朱烔明.大角度單擺的周期［J］.上海師范大學(xué)學(xué)報(bào)，2004，（33）：103-105.

［5］邵偉，凌丹.基于智能積分的改進(jìn)增量式PID算法［J］.機(jī)電工程技術(shù)，2010，（11）.

［6］敬喜.卡爾曼濾波器及其應(yīng)用基礎(chǔ)［M］.北京：國防工業(yè)出版社，1973.

［7］蔣志凱.數(shù)字濾波與卡爾曼濾波［M］.北京：中國科學(xué)技術(shù)出版社，1993．

Design of Wind Pendulum System based on Single Chip

LIN Si-xiang

（Department of Electrical engineering，F(xiàn)ujian Agriculture and Forestry University，F(xiàn)uzhou，F(xiàn)ujian 350002，China）

The paper introduces a kind of wind pose with the single chip processor as the core control system.The system consists of renesas R5F100LEA 16 bits MCU as the control core，using Kalman filtering fusion GY-85 nine shaft freedom of IMU sensor real-time acquisition of wind pendulum pose，through the MCU receives the data，the PID correction，adjusts the PWM duty ratio control fan speed and steering，so as to realize control of the wind swing.Test results show that the design of the wind pendulum system based on single chip microcomputer，can better realize the wind swing of the pendulum，a straight line，circular trajectory，brake static，walk the number 8 shape，and other functions.

R5F100LEA；the sensor；PID；the Kalman filter

TP273

A

2095-980X（2017）01-0054-03

2017-12-14

林思翔（1996-），男，福建莆田人，主要研究方向：電氣工程及其自動化專業(yè)。