覃仕明 玉 函

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

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

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

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

1.1 風(fēng)力擺簡諧運動分析

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

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

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

因此有：

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

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

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

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

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

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

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

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

2 PID 控制算法分析

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

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

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

位置式PID 公式為：

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

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

∑e（k）：代表偏差的累計和。

PWM則代表輸出。

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

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

增量式PID 公式為：

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

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

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

PWM則代表輸出。

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

3 軟件分析

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

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

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

圖2 IIC 通信時序圖

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

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

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

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

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

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

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

4 結(jié)論

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