張國軍， 李紹明， 閆孝姮， 李 威， 朱正印

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

?

風力擺運動控制系統(tǒng)設計

張國軍， 李紹明， 閆孝姮， 李 威， 朱正印

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

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

軸流風機； MPU6050； 運動控制

0 引 言

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

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

1 系統(tǒng)總體方案設計

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

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

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

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

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

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

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

2.1 坐標系的建立

如圖2所示，設風力擺擺桿長度為L，風機高度為H，風力擺自由垂直狀態(tài)下距地面高度為h。當擺動角度α時，激光筆在地面畫出的線段長度為

X=(L+H+h)tanα

(1)

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

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

圖3 風力擺在平面坐標系下示意圖Fig 3 Diagram of wind pendulum in planar coordinate system

因此，風力擺距原點的距離可表示為

(2)

風力擺的方向角度為

(3)

2.2 控制策略

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

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

通過兩個PID環(huán)節(jié)分別對風機運行軌跡的長度和方向進行控制，以保證風力擺能準確按照預設軌跡運動。

圖4 雙PID風力擺控制方案Fig 4 Control scheme of double PID wind pendulum

2.3 直線運動

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

如圖5所示，對風力擺系統(tǒng)進行受力分析可知，在靜止狀態(tài)下風力擺所受的自身重力mg與擺桿對其拉力FT大小相等方向相反，風力擺的運動方向完全由軸流風機輸出的合力FU決定。

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

圖5 風力擺受力分析Fig 5 Force analysis of wind pendulum

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

2.4 圓周運動

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

如圖6所示，風機做圓周運動的實現(xiàn)方式是通過給風機整體施加離心力，使得風機從原點開始以螺旋線式軌跡逐漸逼近到目標圓軌跡。

圖6 風力擺圓周運動示意圖Fig 6 Diagram of circular motion of wind pendulum

控制器通過對當前風力擺所處軌跡半徑與目標圓軌跡半徑比較后，通過PID控制器得出離心力FU,并將其分解分配給相應的風機。

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

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

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

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

3.1 直線運動測試

將白紙板的原點與風力擺平面坐標系下的原點重合，將0°刻度線放置在坐標系的x軸正半軸上。通過人機交互模塊輸入設定的直線軌跡，反復測試。

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

表1 沿0°和30°方向直線運動測試

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

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

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

表2 沿任意方向直線運動測試

3.2 圓周運動測試

為方便測試觀察，在地面放置的白紙板的圓上均勻標注6個圓點。在風力擺做圓周運動時，記錄風力擺擺動3圈內(nèi)激光筆在6個圓點處的軌跡是否在±2.5 cm的圓環(huán)內(nèi)。

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

表3 圓周運動測試

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

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

表4 圓周運動抗干擾測試

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

4 結(jié)束語

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

本系統(tǒng)榮獲2015年全國大學生電子設計競賽國家一等獎。

[1] 全國大學生電子設計競賽組委會.全國大學生電子設計競賽獲獎作品選編[M].北京：北京理工大學出版社，2011.

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

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

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

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

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

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

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

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

[10] 阮曉鋼,李亞磊,朱曉慶.兩輪機器人在坡面上的運動平衡控制[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

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