基于單片機的風力擺系統設計

林思翔

（福建農林大學 電氣工程系，福建 福州 350002）

基于單片機的風力擺系統設計

林思翔

（福建農林大學 電氣工程系，福建 福州 350002）

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

R5F100LEA；傳感器；PID；Kalman濾波

隨著現代測控技術的迅猛發展，以微處理器為基礎，在交通、醫學、化工、機器人、國防等領域廣泛應用。文章提出一種基于單片機的風力擺控制系統，通過GY-85九軸自由度IMU，對擺桿的運行狀態數據進行實時采集并反饋回單片機，形成閉環控制系統。單片機對返回的數據結合基于卡爾曼濾波的PID控制算法進行處理，產生的控制量作用于驅動電路,從而實現對四個軸流風機轉速及轉向的控制。四個軸流風機產生空氣推動各風機使風力擺按照一定規律運動。

1 系統構建

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

圖1 系統總體框圖

2 系統硬件電路

系統采用瑞薩R5F100LEA作主控芯片，基于16位CISC架構，內置高速片上振蕩器時鐘及按鍵中斷，具有超強的模擬量處理功能，支持On-chip調試功能。采用核心板和底板兩級接插方式，方便進行調節與功能轉換。底板包括JTAG電路與相應的濾波穩壓電路，如圖2所示。

圖2 微控制系統核心電路圖

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

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

電機驅動采用MOS管SI2300輸出方式提高帶載能力，反接的二極管IN41478和0.1uf電容起到保護作用，電阻R3用于防止過電壓擊穿MOS管。電機的轉速通過PWM調節占空比來調節，如圖4所示。

圖4 電機驅動模塊

TPS62203DBVR作為高效率同步降壓轉換器，響應速度快，靜態功耗小，如圖5所示。

圖5 加速度計——陀螺儀模塊供電電路

如圖6所示為功能按鍵和LED顯示的簡易電路，按鍵和顯示模塊使風力擺各個功能穩定有序的實現和提示。

圖6 LED顯示電路

3 系統軟件設計

系統軟件設計主要由兩部分組成。主程序主要負責總體程序管理功能，實現人機交互設定。在單片機開機延時初始化后，啟動主程序，通過按鍵掃描選擇相應功能，并顯示指示燈，具體流程圖如圖7所示。

圖7 主程序流程圖

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

圖8 中斷服務子程序流程圖

風力擺的控制采用PID閉環控制方式結構，可寫為等式：

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

其中，KI=Kp·T/TI為積分系數，KD=Kp·TD/T為微分系數。使用PID控制器，控制執行機構實時跟蹤系統輸出，使風力擺滿足各項要求，如圖9所示。

圖9 PID調節

由于加速度計不能區分重力加速度和運動加速度，需用陀螺儀測量角速率，并利用最小二乘法對其進行補償以減少漂移，進而實現較為精確的測量。將陀螺儀的坐標進行換算以投影到加速度計的坐標平面上去。

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）；

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

圖10 三軸位姿波動值

圖11 風力擺受力圖

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

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

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

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

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

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

圖12 風力擺圓周運動示意圖

驅動風力擺實現走數字8的功能，只需令風力擺以某角度快速起擺后，快速改變電機、轉速、轉向，驅動風力擺擺過一定圓弧；再反向直線擺動并走過圓弧，如此反復運行。

4 結語

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

［1］譚浩強.C語言程序設計［M］．北京：清華大學出版社，2012．

［2］趙曉安.MCS-51單片機原理及應用［M］.天津：天津大學出版社，2001.

［3］張毅剛.MCS-51單片應用設計（第3版）［M］.北京：哈爾濱工業大學出版社，2008.

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

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

［6］敬喜.卡爾曼濾波器及其應用基礎［M］.北京：國防工業出版社，1973.

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

Design of Wind Pendulum System based on Single Chip

LIN Si-xiang

（Department of Electrical engineering，Fujian Agriculture and Forestry University，Fuzhou，Fujian 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-），男，福建莆田人，主要研究方向：電氣工程及其自動化專業。