風(fēng)力擺控制系統(tǒng)分析

覃仕明 玉 函

（1、廣西壯族自治區(qū)特種設(shè)備檢驗(yàn)研究院,廣西 南寧530200 2、通力電梯有限公司南寧分公司,廣西 南寧530200）

風(fēng)力擺控制系統(tǒng)用來(lái)控制電機(jī)做單擺形式的運(yùn)動(dòng),其簡(jiǎn)諧運(yùn)動(dòng)規(guī)律可涉及到物理領(lǐng)域的各個(gè)方面,如分子運(yùn)動(dòng)、電磁振蕩等[1]。風(fēng)力擺控制系統(tǒng)是物理研究理論與現(xiàn)代控制理論的結(jié)合,如何通過(guò)現(xiàn)代控制理論實(shí)現(xiàn)物理運(yùn)動(dòng)規(guī)律,對(duì)目前解決實(shí)際工業(yè)上遇到的問(wèn)題有很大的研究意義,在經(jīng)典控制理論和現(xiàn)代控制理論上非常值得探究。

1 控制系統(tǒng)分析

風(fēng)力擺控制系統(tǒng)是一個(gè)伺服隨動(dòng)控制系統(tǒng)[2],伺服隨動(dòng)控制系統(tǒng)實(shí)現(xiàn)的要求是具備快速跟蹤和精確定位,快速跟蹤與伺服帶寬有關(guān)系,帶寬越大,快速性就會(huì)越好,而帶寬主要會(huì)受到硬件的慣性限制,慣性越來(lái)越大,帶寬越來(lái)越窄,所以風(fēng)力擺控制系統(tǒng)的擺桿質(zhì)量大小可影響其快速跟蹤的性能。系統(tǒng)精確定位的實(shí)現(xiàn)則需要機(jī)械結(jié)構(gòu)與硬件搭建合理,同時(shí)在軟件上使用控制算法提高系統(tǒng)的整體抗干擾性。

1.1 風(fēng)力擺簡(jiǎn)諧運(yùn)動(dòng)分析

風(fēng)力擺控制系統(tǒng)是單擺運(yùn)動(dòng)的衍生,單擺運(yùn)動(dòng)屬于簡(jiǎn)諧運(yùn)動(dòng)[3],所以本系統(tǒng)的運(yùn)動(dòng)本質(zhì)為簡(jiǎn)諧運(yùn)動(dòng),其數(shù)學(xué)方程為：

風(fēng)力擺控制系統(tǒng)的所有運(yùn)動(dòng)軌跡都將以簡(jiǎn)諧運(yùn)動(dòng)作為實(shí)現(xiàn)依據(jù)。除此之外,單擺運(yùn)動(dòng)周期方程為：

式中的g 為重力加速度,L 為風(fēng)力擺的擺桿長(zhǎng)度。假設(shè)取L=0.3m,g 取10m/s2。

因此有：

式中的擺桿長(zhǎng)度的選取是相對(duì)較短的,假設(shè)最長(zhǎng)的情況下,當(dāng)選取擺桿長(zhǎng)度為2m,此時(shí)有

所以由式（3）和式（4）得出結(jié)論,單擺周期的范圍大概在1.09s～2.81s 之間,其主要還是取決于擺桿的長(zhǎng)度。

1.2 風(fēng)力擺擺角分析

角度傳感器MPU6050 中的三軸陀螺儀與三軸加速計(jì)測(cè)量出風(fēng)力擺控制系統(tǒng)當(dāng)前位置信息的原始數(shù)據(jù),再通過(guò)自帶的DMP 進(jìn)行數(shù)據(jù)融合與轉(zhuǎn)換,得出X 軸方向角為roll,Y 軸方向角為pitch,Z 軸方向角為yaw,在風(fēng)力擺控制系統(tǒng)中,只利用到了X 軸與Y 軸方向角度進(jìn)行分析計(jì)算,所以只對(duì)這兩個(gè)方向角度分析。

角度roll（pitch）是風(fēng)力擺發(fā)生在X 軸（Y 軸）上的偏轉(zhuǎn)角度,風(fēng)力擺離地面的高度為H,激光點(diǎn)與零點(diǎn)的距離為X（Y）,目前已經(jīng)知道角度roll（pitch）和離地面的高度H,得出：

根據(jù)公式得出的X（Y）就是激光點(diǎn)與零點(diǎn)的距離,也可以說(shuō)是風(fēng)力擺控制系統(tǒng)當(dāng)前狀態(tài)的測(cè)量值,但我們要控制激光點(diǎn)的位置滿足單擺運(yùn)動(dòng)因此要有目標(biāo)值,結(jié)合公式（6）可得出：

在得到目標(biāo)值與測(cè)量值后,導(dǎo)入PID 控制算法,輸出PWM控制電機(jī)轉(zhuǎn)動(dòng)就可以使風(fēng)力擺控制系統(tǒng)每個(gè)方向完成系統(tǒng)要求的簡(jiǎn)諧運(yùn)動(dòng)了。如圖1 中,為風(fēng)力擺擺角圖。

圖1 風(fēng)力擺擺角圖

2 PID 控制算法分析

風(fēng)力擺控制系統(tǒng)需要考慮抗擾動(dòng)能力,如果不做抗干擾算法,風(fēng)力擺的運(yùn)動(dòng)方程將不會(huì)是簡(jiǎn)諧運(yùn)動(dòng)方程,本系統(tǒng)設(shè)計(jì)的最大調(diào)節(jié)時(shí)間是4 秒,也就是風(fēng)力擺控制系統(tǒng)能在4 秒內(nèi)恢復(fù)簡(jiǎn)諧運(yùn)動(dòng)就可。選用經(jīng)典的PID 算法能達(dá)到簡(jiǎn)單且快速的效果,能解決機(jī)械結(jié)構(gòu)搭建不合理或硬件放置不合理產(chǎn)生的誤差。

2.1 位置閉環(huán)控制

位置閉環(huán)控制是根據(jù)傳感器反饋回來(lái)的電機(jī)位置信息,然后與目標(biāo)值進(jìn)行比較,得到偏差量,再經(jīng)過(guò)對(duì)偏差的比例、積分、微分進(jìn)行控制,使偏差量趨向于零的過(guò)程。

位置式PID 公式為：

e（k）：代表本次偏差。

e（k-1）：代表上一次偏差。

∑e（k）：代表偏差的累計(jì)和。

PWM則代表輸出。

2.2 速度閉環(huán)控制

速度閉環(huán)控制是在單位時(shí)間內(nèi)獲取的脈沖數(shù)所測(cè)量電機(jī)速度信息,再與目標(biāo)值進(jìn)行比較,得出控制偏差量,然后對(duì)偏差的PID 進(jìn)行控制,讓偏差趨向于零的過(guò)程。

增量式PID 公式為：

e（k）：代表本次偏差。

e（k-1）：代表上一次偏差。

e（k-2）：代表上上次偏差。

PWM則代表輸出。

根據(jù)對(duì)兩種PID 閉環(huán)控制的對(duì)比分析,由于MPU6050 角度傳感器已經(jīng)能采集到風(fēng)力擺的位置信息,則采用位置式閉環(huán)控制是風(fēng)力擺控制系統(tǒng)最適合的方法。

3 軟件分析

風(fēng)力擺控制系統(tǒng)上電后開(kāi)始執(zhí)行系統(tǒng)初始化,之后進(jìn)行按鍵判斷,選擇風(fēng)力擺控制系統(tǒng)所要執(zhí)行的功能,如擺動(dòng)距離、擺動(dòng)角度、風(fēng)力擺制動(dòng)、擺動(dòng)畫(huà)圓等功能,選擇功能后風(fēng)力擺控制系統(tǒng)開(kāi)始執(zhí)行動(dòng)作,通過(guò)對(duì)功能設(shè)定的目標(biāo)值與當(dāng)前角度傳感器檢測(cè)到的測(cè)量值進(jìn)行比較,得出偏差,之后輸出PWM控制電機(jī)轉(zhuǎn)動(dòng)執(zhí)行動(dòng)作,同時(shí)上位機(jī)端實(shí)時(shí)顯示測(cè)量值與目標(biāo)值的運(yùn)動(dòng)波形軌跡,判斷當(dāng)前測(cè)量時(shí)是否達(dá)到目標(biāo)值,如果未達(dá)到目標(biāo)值則繼續(xù)執(zhí)行動(dòng)作,達(dá)到目標(biāo)值則結(jié)束。

3.1 姿態(tài)檢測(cè)設(shè)計(jì)

風(fēng)力擺控制系統(tǒng)使用的是MPU6050 作為姿態(tài)檢測(cè)的傳感器,首先,它是以IIC 的通信方式與單片機(jī)進(jìn)行數(shù)據(jù)的傳輸,IIC通信是同步通信,收發(fā)雙方需要一條時(shí)鐘線（SCL）來(lái)控制收發(fā)雙方的通信節(jié)奏。數(shù)據(jù)線（SDL）則在始終線的節(jié)奏基礎(chǔ)上進(jìn)行數(shù)據(jù)傳輸。MPU6050 通過(guò)加速度計(jì)和陀螺儀檢測(cè)系統(tǒng)當(dāng)前姿態(tài)得到的原始數(shù)據(jù),然后使用其內(nèi)部的數(shù)字運(yùn)動(dòng)處理器（DMP）,將原始數(shù)據(jù)進(jìn)行姿態(tài)融合解算轉(zhuǎn)換成四元數(shù),此時(shí)再將四元數(shù)數(shù)據(jù)傳遞給單片機(jī),通過(guò)程序計(jì)算得出歐拉角,從而確定風(fēng)力擺的當(dāng)前姿態(tài)。圖2 為IIC 通信時(shí)序圖。

圖2 IIC 通信時(shí)序圖

如圖中Start 部分,當(dāng)SCL 為高電平,SDA 產(chǎn)生一個(gè)下降沿則代表通信開(kāi)始,數(shù)據(jù)傳輸過(guò)程是當(dāng)SCL 低電平時(shí),SDA 才能變化,表示說(shuō)發(fā)送方一定要先保持SCL 為低電平,SDA 才能變化,SDA 高電平時(shí)表明正在發(fā)送一位數(shù)據(jù)。當(dāng)SCL 高電平時(shí),SDA 絕對(duì)不能夠出現(xiàn)變化,因?yàn)樵谶@時(shí),接收方要讀取當(dāng)前SDA 的信號(hào)為高電平還是低電平,所以要確保SDA 的穩(wěn)定,從圖3 中可知道每位數(shù)據(jù)的改變都處于SCL 的低電平處。當(dāng)SCL為高電平時(shí)且SDA 產(chǎn)生一個(gè)上升沿時(shí),說(shuō)明一次通信過(guò)程的結(jié)束,也就是停止位如圖3 中的Stop 部分所示。

MPU6050 是通過(guò)加速度計(jì)和陀螺儀檢測(cè)系統(tǒng)當(dāng)前姿態(tài)得到的原始數(shù)據(jù),然后使用其內(nèi)部的數(shù)字運(yùn)動(dòng)處理器（DMP）,將原始數(shù)據(jù)進(jìn)行姿態(tài)融合解算轉(zhuǎn)換成四元數(shù),此時(shí)再將四元數(shù)數(shù)據(jù)傳遞給單片機(jī),通過(guò)程序計(jì)算得出歐拉角,從而確定風(fēng)力擺的當(dāng)前姿態(tài)。

如圖3 所示,是四元數(shù)轉(zhuǎn)換成歐拉角的轉(zhuǎn)換代碼。

圖3 四元數(shù)轉(zhuǎn)換歐拉角

DMP 解算得出的四元數(shù)原始數(shù)據(jù)存儲(chǔ)在數(shù)組quat[0]～quat[3] 中,q30 是大小為1073741824（2 的30 次方）的一個(gè)常量,q0～q3 是四元數(shù)轉(zhuǎn)換為浮點(diǎn)數(shù)的結(jié)果,將q0～q3 代入轉(zhuǎn)換公式中,計(jì)算出歐拉角,通過(guò)得出的姿態(tài)角,既能夠檢測(cè)出風(fēng)力擺控制系統(tǒng)當(dāng)前的姿態(tài)。

3.2 PID 控制算法軟件設(shè)計(jì)

通過(guò)對(duì)PID 控制算法的理論分析,風(fēng)力擺控制系統(tǒng)采用的是位置PID 閉環(huán)控制,根據(jù)公式,分別對(duì)X 軸,Y 軸設(shè)計(jì)位置PID 閉環(huán)控制,通過(guò)對(duì)value 和Target 兩個(gè)值的差值進(jìn)行PID 算法處理,輸出X 軸與Y 軸的PWM,實(shí)現(xiàn)對(duì)電機(jī)的控制。

4 結(jié)論

通過(guò)對(duì)風(fēng)力擺控制系統(tǒng)簡(jiǎn)諧運(yùn)動(dòng)分析,同時(shí)對(duì)風(fēng)力擺在擺動(dòng)時(shí)的擺角計(jì)算進(jìn)行分析,以及PID 控制算法控制位置和速度的過(guò)程,論文對(duì)風(fēng)力擺控制系統(tǒng)的理論計(jì)算進(jìn)行了詳細(xì)說(shuō)明,為風(fēng)力擺控制系統(tǒng)的實(shí)際實(shí)現(xiàn)奠定了理論基礎(chǔ)。