作者 吳振宇 李勝銘 高元龍 李超 殷殷 大連理工大學 創新創業學院(遼寧 大連 116024)

　　吳振宇(1971-)，男，博士，副教授，研究方向：嵌入式系統、智能控制、機器人等領域。

摘要：本文主要講述利用Coretx-M4單片機控制風力擺運動軌跡的實現方法。系統根據當前加速度、角速度及圖像信息，利用閉環控制算法調節電機，實現風力擺直線擺動、圓周擺動及定點靜止等功能。系統響應速度快，控制精度高，交互操作界面簡單易用，具有良好的交互性。

引言

　　風力擺系統的擺體由風扇構成，通過調整風扇的轉速實現擺動位置及擺動路線的控制，由于擺動機構的滯后性，實現精確控制具有一定難度。通過合理的擺結構設計，并利用擺線理論建模，設計并優化控制策略，利用閉環結構提升擺的軌跡運動準確性。系統對風擺控制參數設定、抗擾因素測試等慣性滯后問題解決具有借鑒意義，同時也為分析該類問題建立了直觀的測試平臺。

1 系統結構

　　本系統硬件部分主要由電源模塊、MCU模塊、姿態采集模塊、電機驅動模塊、風力擺機械部分等組成。機械部分為萬向節懸掛65cm硬質桿，底端連接4個風機構成風機組，中心固定姿態采集模塊。MCU使用I2C協議采集姿態模塊的數據，MCU根據設定值利用PID控制理論，通過控制PWM占空比來控制 4個風機的轉速和方向，實現對風力擺的控制，整體系統結構如圖1所示。

1.1 電源模塊

　　7.2V /2000mAh鎳鎘電池為MCU模塊、傳感器模塊和顯示模塊供電。

　　學生電源輸出6V穩壓，為四路電機驅動供電。

1.2 MCU模塊

　　MCU模塊是核心部分，負責數據處理。有以下功能：

　　負責讀取風擺角度數據，將加速度、角速度信息進行互補濾波和四元數轉化，計算當前風力擺角度信息;

　　負責讀取攝像頭圖像信息，對數據進行去噪點化處理后提取標志物邊緣，計算得出標志物中心點位置;

　　負責控制電機驅動模塊，利用PID閉環控制算法調節電機轉速和方向，實現對風力擺運動軌跡的控制。

1.3 姿態采集模塊

　　姿態采集模塊是整個控制系統的關鍵組成部分，本系統采用整合性六軸陀螺儀加速度計芯片，負責檢測風力擺的加速度角速度信息^[1]。

1.4 電機驅動模塊

　　電機驅動模塊為雙BTN7960組成的H橋電機驅動，根據MCU控制器輸出的PWM信號和方向信息，控制空心杯電機的轉速和方向。

1.5 攝像頭模塊

　　攝像頭模塊是系統的特殊功能部分，為實現擺頭跟隨目標物而設計。攝像頭采集圖像，并對目標物進行識別，進而實現對目標物的跟隨。

2 系統理論分析與計算

2.1 風力擺狀態的測量與計算

　　采用高精度的加速計和陀螺儀MPU6050，不斷采集風力擺姿態數據。MPU6050對陀螺儀和加速度計分別用了三個16位的ADC，將其測量的模擬量轉化為可輸出的數字量，通過DMP處理器讀取測量數據，然后通過I2C總線輸出，得到風擺的姿態角度。

2.2 風力擺控制分析

　　風力擺通過4只空心杯電機提供驅動推力，姿態采集模塊采集風力擺當前姿態角，單片機處理姿態角，調節4個電機PWM的比例，從而控制下一時刻風機工作狀態，實現對于風力擺的控制^[3]。

　　在自由單擺模型中，單擺做簡諧運動的周期跟擺長的平方根成正比，跟重力加速度的平方根成反比，跟振幅、擺球的質量無關^[6]。

(1)

　　風擺擺長確定后，周期也就確定，如圖2所示，根據自由單擺簡諧運動的特性，利用三角函數關系^[4~5]，風擺跟隨自然周期做出單擺的運動，加上X、Y方向，兩個方向運動相位差90度，如圖3所示，風擺就能做出圓周運動。

　　在處理風力擺模型時，可認為是控制每一時刻風力擺的姿態角，從而控制類自由擺運動和圓周運動。當物體離開垂直的平衡位置之后，便會受到重力與懸線的作用合力，驅動重物回復平衡位置。這個力稱之為回復力，公式為。

　　2.3 控制算法的分析

　　本系統采用PID算法來控制風機轉動的速度^[2]。風機開始工作后，姿態采集模塊不斷采集當前風力擺姿態角狀態，并與之前的狀態比較，使得風力擺的運動狀態逐漸趨向于平穩。PID算法控制器由角度比例P、角度誤差積分I和角度微分D組成。

　　其輸入e(t)與輸出U(t)的關系為：

　(2)

　　它的傳遞函數為：

(3)

3 電路與程序設計

3.1 電路設計

　　3.1.1 電源

　　7.2V蓄電池電源經LT1529-5穩壓得到5V電源，再經過兩片LT1085電源芯片穩壓得到兩路3.3V電源，一路單獨供電MCU，一路供電其它外設。主控板電源原理如圖4所示。