風(fēng)力擺運(yùn)動(dòng)控制系統(tǒng)設(shè)計(jì)

張國(guó)軍， 李紹明， 閆孝姮， 李 威， 朱正印

(遼寧工程技術(shù)大學(xué) 電氣與控制工程學(xué)院，遼寧 葫蘆島 125105)

?

風(fēng)力擺運(yùn)動(dòng)控制系統(tǒng)設(shè)計(jì)

張國(guó)軍， 李紹明， 閆孝姮， 李 威， 朱正印

(遼寧工程技術(shù)大學(xué) 電氣與控制工程學(xué)院，遼寧 葫蘆島 125105)

系統(tǒng)以32位的MK60DN256ZVLL10單片機(jī)為核心，通過(guò)運(yùn)動(dòng)處理傳感器MPU6050測(cè)量風(fēng)力擺在三維空間的位置，控制安裝在擺桿底端的4個(gè)軸流風(fēng)機(jī)使得風(fēng)力擺運(yùn)動(dòng)。在風(fēng)力擺正下端置一激光筆，根據(jù)MPU6050的反饋數(shù)值及三角函數(shù)運(yùn)算，運(yùn)用兩個(gè)獨(dú)立PID算法，控制風(fēng)力擺做直線或圓周運(yùn)動(dòng)，且在圓周運(yùn)動(dòng)時(shí)使風(fēng)力擺受到50～60 W臺(tái)扇在水平方向吹風(fēng)的干擾后，可以5 s內(nèi)恢復(fù)運(yùn)動(dòng)軌跡。實(shí)驗(yàn)數(shù)據(jù)表明：風(fēng)力擺能夠在15 s內(nèi)，完成設(shè)定的直線運(yùn)動(dòng)，軌跡誤差小于±0.5 cm；能夠在20 s完成設(shè)定半徑的圓周運(yùn)動(dòng)，軌跡能夠在設(shè)定半徑±2.5 cm 的圓環(huán)內(nèi)。

軸流風(fēng)機(jī)； MPU6050； 運(yùn)動(dòng)控制

0 引 言

全國(guó)大學(xué)生電子設(shè)計(jì)競(jìng)賽的競(jìng)賽規(guī)模日益擴(kuò)大，2015年全國(guó)共有1 097所高校、13 063支隊(duì)伍、39 189名學(xué)生報(bào)名參加此次賽事，在規(guī)模上堪稱歷屆之最。其中控制類題目倍受青睞，占選題總數(shù)的1/3[1]。近年來(lái)，大賽引領(lǐng)控制目標(biāo)從二維向三維空間過(guò)渡，B題風(fēng)力擺控制系統(tǒng)應(yīng)運(yùn)而生[2]。

本文研究的風(fēng)力運(yùn)動(dòng)擺控制系統(tǒng)不僅達(dá)到了賽題的各項(xiàng)要求，同時(shí)可作為一個(gè)典型的實(shí)驗(yàn)對(duì)象，為理論教學(xué)和課外實(shí)踐搭建了良好的操作平臺(tái)[3,4]。系統(tǒng)以32位單片機(jī)MK60DN256ZVLL10為控制核心，通過(guò)MPU6050運(yùn)動(dòng)處理傳感器對(duì)風(fēng)力擺位置信息的判斷，輔以矩陣鍵盤(pán)、液晶顯示屏、激光筆以及軸流風(fēng)機(jī)等電路，實(shí)現(xiàn)了風(fēng)力擺運(yùn)動(dòng)控制系統(tǒng)。

1 系統(tǒng)總體方案設(shè)計(jì)

風(fēng)力擺運(yùn)動(dòng)控制系統(tǒng)主要包括對(duì)風(fēng)力擺的直線軌跡和圓周軌跡運(yùn)動(dòng)的控制。系統(tǒng)主要由控制模塊、檢測(cè)模塊、執(zhí)行機(jī)構(gòu)、聲光顯示以及人機(jī)交互模塊組成，系統(tǒng)總體框圖如圖1所示。

圖1 系統(tǒng)總體框圖Fig 1 Block diagram of whole system

檢測(cè)模塊主要通過(guò)對(duì)風(fēng)力擺擺動(dòng)角度、擺動(dòng)方向的檢測(cè)，來(lái)確定風(fēng)力擺所處的三維空間位置，系統(tǒng)采用九軸數(shù)字運(yùn)動(dòng)傳感器MPU6050作為信號(hào)檢測(cè)模塊。MPU6050內(nèi)部集成了三軸加速度計(jì)、三軸陀螺儀和一個(gè)可擴(kuò)展的數(shù)字運(yùn)動(dòng)處理器(DMP)，可直接通過(guò)IIC協(xié)議讀取MPU6050當(dāng)前空間位置的四元數(shù)，經(jīng)簡(jiǎn)單運(yùn)算即可描述傳感器的空間位置[5]。由于數(shù)據(jù)獲取之后需要對(duì)其測(cè)得的三軸角速度及三軸加速度進(jìn)行融合濾波，傳統(tǒng)的MPU6050需要用主控單片機(jī)對(duì)MPU6050傳回來(lái)的數(shù)據(jù)進(jìn)行多次配置計(jì)算，最終才能得到DMP處理后的數(shù)值，占用MCU資源，數(shù)據(jù)獲取難以實(shí)時(shí)[6]。設(shè)計(jì)采用帶有硬件濾波的MPU6050傳感器，即在原有基礎(chǔ)上增加了一個(gè)MCU，專門(mén)進(jìn)行DMP運(yùn)算，然后將運(yùn)算結(jié)果通過(guò)串口協(xié)議發(fā)至主控。

本系統(tǒng)加入了人機(jī)交互模塊與聲光顯示模塊，在易于操作的同時(shí)使得系統(tǒng)更加人性化。人機(jī)交互模塊由矩陣鍵盤(pán)和LCD液晶顯示屏組成，根據(jù)液晶屏顯示操作提示，通過(guò)矩陣鍵盤(pán)輸入設(shè)定的參數(shù)及功能。在風(fēng)力擺底端固定一只激光筆，可以方便觀察風(fēng)力擺的運(yùn)動(dòng)軌跡，并在達(dá)到預(yù)設(shè)功能時(shí)通過(guò)蜂鳴器發(fā)出聲響，提示風(fēng)力擺系統(tǒng)的運(yùn)行情況。

控制算法平臺(tái)借助32位MK60DN256ZVLL10單片機(jī)實(shí)現(xiàn)，該單片機(jī)運(yùn)行頻率最大可達(dá)250 MHz，特別適用于賽題中風(fēng)力擺運(yùn)行的高速采樣的要求，其較為充足的I/O更適合硬件電路的設(shè)計(jì)要求[7]。

2 系統(tǒng)理論分析與運(yùn)算

本系統(tǒng)的機(jī)械平臺(tái)如圖2所示，包括支架、軸流風(fēng)機(jī)、碳素?cái)[桿以及萬(wàn)向節(jié)構(gòu)成。為保證風(fēng)力擺運(yùn)動(dòng)軌跡的任意性，采用4個(gè)軸流風(fēng)機(jī)環(huán)形相接作為系統(tǒng)的執(zhí)行機(jī)構(gòu)，4個(gè)方向皆可提供動(dòng)力，且容易實(shí)現(xiàn)預(yù)設(shè)功能。將MPU6050傳感器水平放置于4個(gè)軸流風(fēng)機(jī)的中心，并保證MPU6050傳感器的軸向與風(fēng)機(jī)輸出風(fēng)力方向正交或平行。

2.1 坐標(biāo)系的建立

如圖2所示，設(shè)風(fēng)力擺擺桿長(zhǎng)度為L(zhǎng)，風(fēng)機(jī)高度為H，風(fēng)力擺自由垂直狀態(tài)下距地面高度為h。當(dāng)擺動(dòng)角度α?xí)r，激光筆在地面畫(huà)出的線段長(zhǎng)度為

X=(L+H+h)tanα

(1)

圖2 系統(tǒng)機(jī)械平臺(tái)示意圖Fig 2 Diagram of system mechanical platform

如圖3所示為風(fēng)力擺運(yùn)動(dòng)控制系統(tǒng)在降維處理后所建立的平面直角坐標(biāo)系示意圖。坐標(biāo)系以風(fēng)力擺靜止?fàn)顟B(tài)激光點(diǎn)為坐標(biāo)原點(diǎn)，以支架橫桿為x軸，以垂直支架橫桿為y軸。則MPU6050經(jīng)DMP后得到四元數(shù)中，表征其在x軸和y軸方向上對(duì)地夾角的數(shù)據(jù)，便可以直接作為風(fēng)力擺在平面直角坐標(biāo)系下的坐標(biāo)(x,y)了。

圖3 風(fēng)力擺在平面坐標(biāo)系下示意圖Fig 3 Diagram of wind pendulum in planar coordinate system

因此，風(fēng)力擺距原點(diǎn)的距離可表示為

(2)

風(fēng)力擺的方向角度為

(3)

2.2 控制策略

根據(jù)題目要求，可以總結(jié)出整個(gè)系統(tǒng)的基本特征：直線運(yùn)動(dòng)；圓周運(yùn)動(dòng)；抗擾能力。為達(dá)到這三個(gè)基本要求，選用工業(yè)控制中最常用的PID控制算法。

利用誤差的比例、積分、微分三個(gè)環(huán)節(jié)的組合計(jì)算出控制量[8]。單片機(jī)在每一個(gè)采樣周期中采集來(lái)自傳感器的風(fēng)機(jī)映射的平面位置，與期望值進(jìn)行比較后，驅(qū)動(dòng)軸流風(fēng)機(jī)實(shí)現(xiàn)對(duì)風(fēng)力擺的實(shí)時(shí)控制。由于系統(tǒng)搭建的平臺(tái)并非理想化，存在過(guò)多的干擾因素，因此，在理論計(jì)算分析后，利用試湊法通過(guò)大量的實(shí)驗(yàn)來(lái)確定PID控制器的比例、積分和微分參數(shù)[9]。本文采用的兩個(gè)獨(dú)立的PID控制方案如圖4所示。

通過(guò)兩個(gè)PID環(huán)節(jié)分別對(duì)風(fēng)機(jī)運(yùn)行軌跡的長(zhǎng)度和方向進(jìn)行控制，以保證風(fēng)力擺能準(zhǔn)確按照預(yù)設(shè)軌跡運(yùn)動(dòng)。

圖4 雙PID風(fēng)力擺控制方案Fig 4 Control scheme of double PID wind pendulum

2.3 直線運(yùn)動(dòng)

控制要求：可設(shè)定擺動(dòng)方向，風(fēng)力擺從靜止開(kāi)始，15 s內(nèi)按照設(shè)置的任意角度擺動(dòng)，畫(huà)出不短于20 cm的直線段。

如圖5所示，對(duì)風(fēng)力擺系統(tǒng)進(jìn)行受力分析可知，在靜止?fàn)顟B(tài)下風(fēng)力擺所受的自身重力mg與擺桿對(duì)其拉力FT大小相等方向相反，風(fēng)力擺的運(yùn)動(dòng)方向完全由軸流風(fēng)機(jī)輸出的合力FU決定。

由于風(fēng)力擺的四個(gè)軸流風(fēng)機(jī)均與坐標(biāo)軸正交擺放，風(fēng)力輸出方向皆平行或垂直于坐標(biāo)軸，故可將FU分解為x,y兩個(gè)方向上的分力其中Fx=FUcosθ，F(xiàn)y=FUsinθ。

圖5 風(fēng)力擺受力分析Fig 5 Force analysis of wind pendulum

由此，在控制風(fēng)力擺系統(tǒng)直線運(yùn)動(dòng)時(shí)，通過(guò)對(duì)風(fēng)力擺位置與目標(biāo)位置間關(guān)系的判斷，經(jīng)PID控制器得出風(fēng)力擺整體輸出合力FU，將其分解為Fx,Fy，并作為控制量分配給相應(yīng)的風(fēng)機(jī)。例如，目標(biāo)位置在第一象限時(shí)，F(xiàn)x為風(fēng)機(jī)4的輸出量，F(xiàn)y為風(fēng)機(jī)3的輸出量。

2.4 圓周運(yùn)動(dòng)

控制要求：以風(fēng)力擺靜止時(shí)激光筆的光點(diǎn)為圓心，驅(qū)動(dòng)風(fēng)力擺用激光筆在地面畫(huà)圓，30 s內(nèi)需重復(fù) 3 次；圓半徑可在 15～35 cm 范圍內(nèi)設(shè)置，激光筆畫(huà)出的軌跡應(yīng)落在指定半徑±2.5 cm 的圓環(huán)內(nèi)。

如圖6所示，風(fēng)機(jī)做圓周運(yùn)動(dòng)的實(shí)現(xiàn)方式是通過(guò)給風(fēng)機(jī)整體施加離心力，使得風(fēng)機(jī)從原點(diǎn)開(kāi)始以螺旋線式軌跡逐漸逼近到目標(biāo)圓軌跡。

圖6 風(fēng)力擺圓周運(yùn)動(dòng)示意圖Fig 6 Diagram of circular motion of wind pendulum

控制器通過(guò)對(duì)當(dāng)前風(fēng)力擺所處軌跡半徑與目標(biāo)圓軌跡半徑比較后，通過(guò)PID控制器得出離心力FU,并將其分解分配給相應(yīng)的風(fēng)機(jī)。

以風(fēng)機(jī)從第一象限向逆時(shí)針?lè)较蜻\(yùn)動(dòng)為例，根據(jù)目標(biāo)圓軌跡及風(fēng)機(jī)當(dāng)前位置確定風(fēng)機(jī)輸出風(fēng)力FU，將其分解為兩個(gè)軸向上的分力，其中，F(xiàn)x=FUcos(θ+90°)，F(xiàn)y=FUsin(θ+90°)。此時(shí)，F(xiàn)x為風(fēng)機(jī)2的輸出量，F(xiàn)y為風(fēng)機(jī)3的輸出量。當(dāng)不同象限函數(shù)值有負(fù)號(hào)出現(xiàn)時(shí)，程序中通過(guò)判斷選擇相應(yīng)的風(fēng)機(jī)輸出。

3 測(cè)試結(jié)果與分析

為了驗(yàn)證上述方法的可靠性，本次研究分別為直線運(yùn)動(dòng)和圓周運(yùn)動(dòng)設(shè)計(jì)了相應(yīng)的測(cè)試方法，通過(guò)大量的實(shí)驗(yàn)測(cè)試，在整定PID控制器各項(xiàng)參數(shù)的同時(shí)也檢驗(yàn)了控制方法。

為方便測(cè)試，在地面上鋪設(shè)一張畫(huà)有方向刻度和設(shè)定半徑的圓的白紙板，觀察激光筆與設(shè)定軌跡間的位置情況，記錄誤差并加以分析。

3.1 直線運(yùn)動(dòng)測(cè)試

將白紙板的原點(diǎn)與風(fēng)力擺平面坐標(biāo)系下的原點(diǎn)重合，將0°刻度線放置在坐標(biāo)系的x軸正半軸上。通過(guò)人機(jī)交互模塊輸入設(shè)定的直線軌跡，反復(fù)測(cè)試。

在不同的方向角度下，風(fēng)力擺做直線運(yùn)動(dòng)的情況不盡相同。表1是分別對(duì)運(yùn)動(dòng)軌跡的方向角度為0°和30°時(shí)進(jìn)行測(cè)試，并記錄了有代表性的數(shù)據(jù)結(jié)果。

表1 沿0°和30°方向直線運(yùn)動(dòng)測(cè)試

如表1所示，其結(jié)果表明：在這兩種情況下，風(fēng)力擺系統(tǒng)均可以在15 s內(nèi)完成50 cm以上的直線，且誤差范圍均在1 cm以內(nèi)。但在沿30°方向直線運(yùn)動(dòng)時(shí)完成時(shí)間要明顯大于沿0°方向運(yùn)動(dòng)。

由于沿30°方向的直線運(yùn)動(dòng)是由兩個(gè)風(fēng)機(jī)輸出的合力組成，在力的合成過(guò)程中存在損失，因此,可適當(dāng)增加該情況下的PID控制器的P參數(shù)[10]。

在測(cè)試風(fēng)力擺沿任意方向直線運(yùn)動(dòng)時(shí)設(shè)定直線長(zhǎng)度為40 cm，表2所示為該情況下的測(cè)試數(shù)據(jù)，結(jié)果表明：風(fēng)力擺能夠在15 s內(nèi)沿設(shè)定方向畫(huà)出不短于20 cm的直線。但在方向角度為90°～180°間的角度誤差偏大，可能由于風(fēng)機(jī)的擺放位置并非絕對(duì)對(duì)稱平行，導(dǎo)致風(fēng)力的合成存在偏差。

表2 沿任意方向直線運(yùn)動(dòng)測(cè)試

3.2 圓周運(yùn)動(dòng)測(cè)試

為方便測(cè)試觀察，在地面放置的白紙板的圓上均勻標(biāo)注6個(gè)圓點(diǎn)。在風(fēng)力擺做圓周運(yùn)動(dòng)時(shí)，記錄風(fēng)力擺擺動(dòng)3圈內(nèi)激光筆在6個(gè)圓點(diǎn)處的軌跡是否在±2.5 cm的圓環(huán)內(nèi)。

表3所示為風(fēng)力擺靜止開(kāi)始，做圓周運(yùn)動(dòng)時(shí)的測(cè)試數(shù)據(jù)。結(jié)果表明：可以在20 s內(nèi)完成風(fēng)力擺的圓周運(yùn)動(dòng)，且在圓弧半徑小于30 cm的情況下完成較好，在半徑為35 cm時(shí)，運(yùn)動(dòng)軌跡會(huì)存在小范圍內(nèi)的變形。

表3 圓周運(yùn)動(dòng)測(cè)試

由于所選軸流風(fēng)機(jī)的輸出有限且風(fēng)機(jī)質(zhì)量較大，在圓弧半徑較大時(shí)，風(fēng)機(jī)輸出的風(fēng)力不足以克服重力分量，因而運(yùn)動(dòng)軌跡會(huì)發(fā)生較小的變形。

為了檢驗(yàn)風(fēng)力擺運(yùn)動(dòng)控制系統(tǒng)圓周運(yùn)動(dòng)的抗擾性，在風(fēng)力擺達(dá)到預(yù)設(shè)圓周運(yùn)動(dòng)軌跡后，通過(guò)一臺(tái)50～60 W的臺(tái)扇在水平方向吹風(fēng)對(duì)其進(jìn)行干擾。撤出干擾后5 s，再進(jìn)行測(cè)試。測(cè)試結(jié)果如表4所示。

表4 圓周運(yùn)動(dòng)抗干擾測(cè)試

根據(jù)表4的測(cè)試結(jié)果表明：風(fēng)力擺系統(tǒng)在做圓周運(yùn)動(dòng)時(shí)，可以在施加干擾后5 s恢復(fù)圓周運(yùn)動(dòng)，且圓周運(yùn)動(dòng)的完成情況與施加干擾前一樣。

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

本文設(shè)計(jì)實(shí)現(xiàn)了基于單片機(jī)MK60DN256ZVLL10的風(fēng)力擺運(yùn)動(dòng)控制系統(tǒng)，選擇傳感器MPU6050作為檢測(cè)模塊，并成功將三維空間問(wèn)題降維到二維空間處理。根據(jù)風(fēng)力擺系統(tǒng)抗擾的控制要求，采用兩個(gè)獨(dú)立PID控制策略，系統(tǒng)可以在指定時(shí)間內(nèi)完成預(yù)設(shè)運(yùn)動(dòng)軌跡。對(duì)系統(tǒng)進(jìn)行了詳盡的測(cè)試與分析，測(cè)試結(jié)果表明，風(fēng)力擺系統(tǒng)可以在控制要求范圍內(nèi)達(dá)到理想的效果，如進(jìn)一步提高精度，則有待進(jìn)一步對(duì)系統(tǒng)進(jìn)行改進(jìn)。

本系統(tǒng)榮獲2015年全國(guó)大學(xué)生電子設(shè)計(jì)競(jìng)賽國(guó)家一等獎(jiǎng)。

[1] 全國(guó)大學(xué)生電子設(shè)計(jì)競(jìng)賽組委會(huì).全國(guó)大學(xué)生電子設(shè)計(jì)競(jìng)賽獲獎(jiǎng)作品選編[M].北京：北京理工大學(xué)出版社，2011.

[2] 胡仁杰,堵國(guó)樑.2013年江蘇賽區(qū)全國(guó)大學(xué)生電子設(shè)計(jì)競(jìng)賽優(yōu)秀作品設(shè)計(jì)報(bào)告選編[M].南京：東南大學(xué)出版社，2014.

[3] 張少如,閆若穎,孟慶申,等.電子設(shè)計(jì)競(jìng)賽與大學(xué)生創(chuàng)新創(chuàng)業(yè)能力培養(yǎng)研究[J].勞動(dòng)保障世界,2015(18):7.

[4] 郝立果,劉有濤,陳文亮,等.基于自由擺的激光定位控制系統(tǒng)[J].電子測(cè)量與儀器學(xué)報(bào),2013,27(4):381-386.

[5] 李秀豐,郭立泉,王計(jì)平,等.基于六軸傳感器的步態(tài)分析系統(tǒng)設(shè)計(jì)[J].傳感器與微系統(tǒng),2014,33(7):89-91.

[6] 林生榮,張 輝.三軸加速度傳感器校正方法研究[J].傳感器與微系統(tǒng),2011,30(11):72-74,78.

[7] 朱樂(lè)樂(lè).基于ARM Cortex—M4內(nèi)核的多功能教育機(jī)器人開(kāi)發(fā)研究[D].蘇州：蘇州大學(xué),2013.

[8] 宿敬亞,樊鵬輝,蔡開(kāi)元.四旋翼飛行器的非線性PID姿態(tài)控制[J].北京航空航天大學(xué)學(xué)報(bào)，2011,37(9):1054-1058.

[9] 王啟源,阮曉鋼.獨(dú)輪自平衡機(jī)器人雙閉環(huán)非線性PID控制[J].控制與決策,2012,27(4):593-597.

[10] 阮曉鋼,李亞磊,朱曉慶.兩輪機(jī)器人在坡面上的運(yùn)動(dòng)平衡控制[J].傳感器與微系統(tǒng),2014,33(3):44-47.

李紹明，通訊作者，E—mail：630277391@qq.com。

Design of motion control system for wind pendulum

ZHANG Guo-jun, LI Shao-ming, YAN Xiao-heng, LI Wei, ZHU Zheng-yin

(School of Electrical and Control Engineering,Liaoning Technical University,Huludao 125105,China)

The system controls four axial flow fan which mounted in the bottom of the pendulum in order to make the wind pendulum move 32 bit MK60DN256ZVLL10 microcontroller is used as core;position of wind pendulum placed in three-dimensional space is measured through motion processing sensor MPU6050.There is a laser pointer under the wind pendulum.The wind pendulum can do linear or circular motion according to the feedback value and trigonometric functions computation with two independent PID algorithm.The PID algorithm can make the wind pendulum when it is disturbed by 50～60 W FANS recovery circular motion within 5 s.Experimental results show that the wind pendulum completes predetermined linear motion with trajectory errors less than ±0.5 cm within 15 s and circular motion within diameter ±2.5 cm,within 20 s.

axial flow fan; MPU6050; motion control

10.13873/J.1000—9787(2016)12—0106—04

2016—01—22

TP 272

A

1000—9787(2016)12—0106—04

張國(guó)軍(1960-)，男，黑龍江大慶人，教授，全國(guó)大學(xué)生電子設(shè)計(jì)競(jìng)賽遼寧專家組成員，從事電氣工程與智能控制研究工作。