作者 吳振宇 李勝銘 高元龍 李超 殷殷 大連理工大學(xué) 創(chuàng)新創(chuàng)業(yè)學(xué)院(遼寧 大連 116024)

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

摘要：本文主要講述利用Coretx-M4單片機(jī)控制風(fēng)力擺運(yùn)動(dòng)軌跡的實(shí)現(xiàn)方法。系統(tǒng)根據(jù)當(dāng)前加速度、角速度及圖像信息，利用閉環(huán)控制算法調(diào)節(jié)電機(jī)，實(shí)現(xiàn)風(fēng)力擺直線擺動(dòng)、圓周擺動(dòng)及定點(diǎn)靜止等功能。系統(tǒng)響應(yīng)速度快，控制精度高，交互操作界面簡(jiǎn)單易用，具有良好的交互性。

引言

　　風(fēng)力擺系統(tǒng)的擺體由風(fēng)扇構(gòu)成，通過(guò)調(diào)整風(fēng)扇的轉(zhuǎn)速實(shí)現(xiàn)擺動(dòng)位置及擺動(dòng)路線的控制，由于擺動(dòng)機(jī)構(gòu)的滯后性，實(shí)現(xiàn)精確控制具有一定難度。通過(guò)合理的擺結(jié)構(gòu)設(shè)計(jì)，并利用擺線理論建模，設(shè)計(jì)并優(yōu)化控制策略，利用閉環(huán)結(jié)構(gòu)提升擺的軌跡運(yùn)動(dòng)準(zhǔn)確性。系統(tǒng)對(duì)風(fēng)擺控制參數(shù)設(shè)定、抗擾因素測(cè)試等慣性滯后問(wèn)題解決具有借鑒意義，同時(shí)也為分析該類(lèi)問(wèn)題建立了直觀的測(cè)試平臺(tái)。

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

　　本系統(tǒng)硬件部分主要由電源模塊、MCU模塊、姿態(tài)采集模塊、電機(jī)驅(qū)動(dòng)模塊、風(fēng)力擺機(jī)械部分等組成。機(jī)械部分為萬(wàn)向節(jié)懸掛65cm硬質(zhì)桿，底端連接4個(gè)風(fēng)機(jī)構(gòu)成風(fēng)機(jī)組，中心固定姿態(tài)采集模塊。MCU使用I2C協(xié)議采集姿態(tài)模塊的數(shù)據(jù)，MCU根據(jù)設(shè)定值利用PID控制理論，通過(guò)控制PWM占空比來(lái)控制 4個(gè)風(fēng)機(jī)的轉(zhuǎn)速和方向，實(shí)現(xiàn)對(duì)風(fēng)力擺的控制，整體系統(tǒng)結(jié)構(gòu)如圖1所示。

1.1 電源模塊

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

　　學(xué)生電源輸出6V穩(wěn)壓，為四路電機(jī)驅(qū)動(dòng)供電。

1.2 MCU模塊

　　MCU模塊是核心部分，負(fù)責(zé)數(shù)據(jù)處理。有以下功能：

　　負(fù)責(zé)讀取風(fēng)擺角度數(shù)據(jù)，將加速度、角速度信息進(jìn)行互補(bǔ)濾波和四元數(shù)轉(zhuǎn)化，計(jì)算當(dāng)前風(fēng)力擺角度信息;

　　負(fù)責(zé)讀取攝像頭圖像信息，對(duì)數(shù)據(jù)進(jìn)行去噪點(diǎn)化處理后提取標(biāo)志物邊緣，計(jì)算得出標(biāo)志物中心點(diǎn)位置;

　　負(fù)責(zé)控制電機(jī)驅(qū)動(dòng)模塊，利用PID閉環(huán)控制算法調(diào)節(jié)電機(jī)轉(zhuǎn)速和方向，實(shí)現(xiàn)對(duì)風(fēng)力擺運(yùn)動(dòng)軌跡的控制。

1.3 姿態(tài)采集模塊

　　姿態(tài)采集模塊是整個(gè)控制系統(tǒng)的關(guān)鍵組成部分，本系統(tǒng)采用整合性六軸陀螺儀加速度計(jì)芯片，負(fù)責(zé)檢測(cè)風(fēng)力擺的加速度角速度信息^[1]。

1.4 電機(jī)驅(qū)動(dòng)模塊

　　電機(jī)驅(qū)動(dòng)模塊為雙BTN7960組成的H橋電機(jī)驅(qū)動(dòng)，根據(jù)MCU控制器輸出的PWM信號(hào)和方向信息，控制空心杯電機(jī)的轉(zhuǎn)速和方向。

1.5 攝像頭模塊

　　攝像頭模塊是系統(tǒng)的特殊功能部分，為實(shí)現(xiàn)擺頭跟隨目標(biāo)物而設(shè)計(jì)。攝像頭采集圖像，并對(duì)目標(biāo)物進(jìn)行識(shí)別，進(jìn)而實(shí)現(xiàn)對(duì)目標(biāo)物的跟隨。

2 系統(tǒng)理論分析與計(jì)算

2.1 風(fēng)力擺狀態(tài)的測(cè)量與計(jì)算

　　采用高精度的加速計(jì)和陀螺儀MPU6050，不斷采集風(fēng)力擺姿態(tài)數(shù)據(jù)。MPU6050對(duì)陀螺儀和加速度計(jì)分別用了三個(gè)16位的ADC，將其測(cè)量的模擬量轉(zhuǎn)化為可輸出的數(shù)字量，通過(guò)DMP處理器讀取測(cè)量數(shù)據(jù)，然后通過(guò)I2C總線輸出，得到風(fēng)擺的姿態(tài)角度。

2.2 風(fēng)力擺控制分析

　　風(fēng)力擺通過(guò)4只空心杯電機(jī)提供驅(qū)動(dòng)推力，姿態(tài)采集模塊采集風(fēng)力擺當(dāng)前姿態(tài)角，單片機(jī)處理姿態(tài)角，調(diào)節(jié)4個(gè)電機(jī)PWM的比例，從而控制下一時(shí)刻風(fēng)機(jī)工作狀態(tài)，實(shí)現(xiàn)對(duì)于風(fēng)力擺的控制^[3]。

　　在自由單擺模型中，單擺做簡(jiǎn)諧運(yùn)動(dòng)的周期跟擺長(zhǎng)的平方根成正比，跟重力加速度的平方根成反比，跟振幅、擺球的質(zhì)量無(wú)關(guān)^[6]。

(1)

　　風(fēng)擺擺長(zhǎng)確定后，周期也就確定，如圖2所示，根據(jù)自由單擺簡(jiǎn)諧運(yùn)動(dòng)的特性，利用三角函數(shù)關(guān)系^[4~5]，風(fēng)擺跟隨自然周期做出單擺的運(yùn)動(dòng)，加上X、Y方向，兩個(gè)方向運(yùn)動(dòng)相位差90度，如圖3所示，風(fēng)擺就能做出圓周運(yùn)動(dòng)。

　　在處理風(fēng)力擺模型時(shí)，可認(rèn)為是控制每一時(shí)刻風(fēng)力擺的姿態(tài)角，從而控制類(lèi)自由擺運(yùn)動(dòng)和圓周運(yùn)動(dòng)。當(dāng)物體離開(kāi)垂直的平衡位置之后，便會(huì)受到重力與懸線的作用合力，驅(qū)動(dòng)重物回復(fù)平衡位置。這個(gè)力稱之為回復(fù)力，公式為。

　　2.3 控制算法的分析

　　本系統(tǒng)采用PID算法來(lái)控制風(fēng)機(jī)轉(zhuǎn)動(dòng)的速度^[2]。風(fēng)機(jī)開(kāi)始工作后，姿態(tài)采集模塊不斷采集當(dāng)前風(fēng)力擺姿態(tài)角狀態(tài)，并與之前的狀態(tài)比較，使得風(fēng)力擺的運(yùn)動(dòng)狀態(tài)逐漸趨向于平穩(wěn)。PID算法控制器由角度比例P、角度誤差積分I和角度微分D組成。

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

　(2)

　　它的傳遞函數(shù)為：

(3)

3 電路與程序設(shè)計(jì)

3.1 電路設(shè)計(jì)

　　3.1.1 電源

　　7.2V蓄電池電源經(jīng)LT1529-5穩(wěn)壓得到5V電源，再經(jīng)過(guò)兩片LT1085電源芯片穩(wěn)壓得到兩路3.3V電源，一路單獨(dú)供電MCU，一路供電其它外設(shè)。主控板電源原理如圖4所示。