一種基于模糊PID的立方體機器人控制系統(tǒng)①

張艦棟 章 政 黃衛(wèi)華 李 磊 陳 陽

(*武漢科技大學(xué)機器人與智能系統(tǒng)研究院 武漢430081)

(**武漢科技大學(xué)冶金自動化與檢測技術(shù)教育部工程研究中心 武漢430081)

(***武漢科技大學(xué)信息科學(xué)與工程學(xué)院 武漢430081)

0 引言

立方體機器人是近20 年出現(xiàn)的一種結(jié)構(gòu)和運動方式獨特的新型機器人[1-3],其驅(qū)動電機、控制系統(tǒng)、傳感器以及電源等模塊均包含在立方體內(nèi)部,具有全封閉[3-4]或半封閉[5-6]的立方體外殼。與傳統(tǒng)的輪式、腿式、履帶式等機器人相比,立方體機器人具有結(jié)構(gòu)簡單、外觀新穎和組裝方式靈活多變等特點,在太空探索、編隊表演、未來無人機平衡控制等領(lǐng)域具有廣闊的應(yīng)用前景。

立方體機器人通過安裝在立方體內(nèi)部某個面的動量輪的旋轉(zhuǎn)產(chǎn)生重心偏移,基于動量守恒完成立方體機器人的單棱邊、單點自平衡或翻滾等可控運動,可以等效為一個多自由度空間動量輪倒立擺系統(tǒng)[7-9]。此外,受限于立方體機器人獨特的外觀結(jié)構(gòu)和運動方式,現(xiàn)有的立方體機器人缺乏直接的外部環(huán)境測量傳感器,導(dǎo)致難以獲取精確的姿態(tài)數(shù)據(jù),存在抗干擾能力較差、控制精度不高等問題。因此,動力學(xué)建模和自平衡控制一直是立方體機器人的研究重點和難點。文獻(xiàn)[1]設(shè)計了一種金屬殼和一個裝有動力擺機構(gòu)組成的立方體系統(tǒng),采用模糊控制方法設(shè)計了一種基于Sugeno 模糊推理型智能積分模糊平衡控制器,可實現(xiàn)以棱邊為支點的平衡。文獻(xiàn)[3,4]設(shè)計了一種由內(nèi)部3 個兩兩相交垂直的動量輪和驅(qū)動電機組成的立方體機器人裝置,對立方體機器人以棱邊和角點為支點的平衡問題做了較系統(tǒng)的分析,并對立方體機器人實現(xiàn)以角點為支點平衡的原理進(jìn)行了介紹。文獻(xiàn)[5]先采用拉格朗日方法和凱恩方法建立了立方體機器人的單邊動力學(xué)模型,通過比較兩種方法的建模結(jié)果和數(shù)值仿真分析,從理論上驗證了所建立模型的正確性。文獻(xiàn)[6]針對立方體機器人單點平衡,設(shè)計了自抗擾控制器實現(xiàn)了立方體機器人的單點平衡控制。

上述研究主要是研究立方體機器人在平面上的建模和控制,本文針對立方體機器人坡面自平衡控制系統(tǒng)進(jìn)行研究。

考慮到立方體機器人具有非線性、欠驅(qū)動、強耦合、多變量等復(fù)雜特性,難以獲取其精確的數(shù)學(xué)模型,并且受限于其特殊的自平衡方式,采用經(jīng)典的控制方法存在控制精度不高、抗干擾能力較差等問題。因此,本文將模糊邏輯用于立方體機器人控制系統(tǒng)中,實現(xiàn)其在坡面自平衡控制。首先,基于自主設(shè)計的一種內(nèi)部集成的單動量輪棱邊平衡立方體機器人,采用拉格朗日法建立了以棱邊平衡于坡面的立方體機器人動力學(xué)模型,并對立方體機器人的穩(wěn)定性、能控性及能觀性進(jìn)行分析。然后,設(shè)計了一種基于模糊PID的角速度-角度串級控制系統(tǒng)。最后,仿真和實測實驗驗證了所設(shè)計系統(tǒng)的動、靜態(tài)特性。

1 立方體機器人機械結(jié)構(gòu)設(shè)計

在保證立方體機器人結(jié)構(gòu)強度可靠性以及功能完整性的情況下,本文設(shè)計了一種單面安裝動量輪內(nèi)部集成的立方體機器人,由立方體本體、一面上的動量輪、直流電機、傳感器、供電模塊以及主控單元構(gòu)成。立方體機器人結(jié)構(gòu)如圖1 所示。

圖1 立方體機器人結(jié)構(gòu)圖

立方體機器人的主控單元采用32 位低功耗ARM 微控制器,動量輪直徑為0.08 m,姿態(tài)觀測傳感器選用MPU 6050,完成對立方體姿態(tài)數(shù)據(jù)的實時采集,采用陀螺儀/加速度計互補濾波得到立方體機器人傾角最優(yōu)估計。立方體機器人的主要參數(shù)以及數(shù)據(jù)如表1 所示。

表1 立方體機器人的主要參數(shù)表

2 立方體機器人坡面平衡建模

立方體機器人以一條棱邊為支點在斜面上的動態(tài)平衡等效模型可視作為一階倒立擺模型。為了便于簡化立方體機器人建模與控制器設(shè)計,本文對立方體機器人做出如下假設(shè)。

(1) 立方體機器人以棱邊為支點的所在邊與接觸面無滑動。

(2) 立方體機器人視為剛體且質(zhì)量均勻?qū)ΨQ。

(3) 忽略空氣阻力以及各種摩擦力。

立方體機器人以一條單棱邊為支點在坡面上平衡時,由電機驅(qū)動動量輪產(chǎn)生回復(fù)力實現(xiàn)平衡控制,因此立方體機器人的系統(tǒng)模型可簡化為其平面二維模型,如圖2 所示。立方體機器人位于傾角為γ的坡面上,其中M為立方體框架及電池的總質(zhì)量,其重心G的位置坐標(biāo)為(x1,y1)。立方體機器人與坡面接觸點記為點A,點A正下方與水平面接觸點記為坐標(biāo)系原點O,立方體繞點A進(jìn)行轉(zhuǎn)動,轉(zhuǎn)動慣量為I1,m為動量輪的質(zhì)量,其重心P的位置坐標(biāo)為(x2,y2),動量輪轉(zhuǎn)過的角度為β,轉(zhuǎn)動慣量為I2。立方體機器人相對于縱軸的偏轉(zhuǎn)角為θ,點A與立方體機器人重心G的距離為l1,與動量輪重心P的距離為l2,與坡面底點B的距離為l3。

圖2 立方體機器人在坡面上平衡的坐標(biāo)系定義

立方體機器人在坡面上平衡的拉格朗日方程為

式中,L為拉格朗日算子,q為系統(tǒng)的廣義坐標(biāo),T為系統(tǒng)的總動能,V為系統(tǒng)的總勢能。

拉格朗日方程由廣義坐標(biāo)qi以及拉格朗日算子L所表示為

式中,i=1,2,3…n;Qi為系統(tǒng)沿廣義坐標(biāo)方向的外力,設(shè)立方體機器人的2 個廣義坐標(biāo)分別是θ和β。

立方體機器人在坡面上的總動能為

其中,立方體機器人機體的動能為

動量輪的動能為

則立方體機器人在坡面上平衡的總動能為

立方體機器人在坡面上的總勢能為

其中,立方體機器人機體的勢能為

動量輪的勢能為

則立方體機器人在坡面上平衡的總勢能為

由此可得,立方體機器人在坡面上平衡的拉格朗日算子為

立方體機器人在坡面上平衡的廣義力矩為

式中,U為動量輪的驅(qū)動力矩。

將式(11)、(12)帶入式(2),則有:

由此可得:

將式(14)代入式(2),則有:

由此可得立方體機器人在坡面上平衡的動力學(xué)模型為

為了便于分析和計算,將式(16)表示為

當(dāng)立方體機器人處于平衡位置時,各個狀態(tài)變量的初始值為0,即:

3 基于模糊PID的立方體機器人串級控制系統(tǒng)設(shè)計

穩(wěn)定性是控制系統(tǒng)正常工作的必要條件,對于式(19)所示的立方體機器人系統(tǒng),其特征根分別為λ1=0、λ2=8.1521、λ3=-8.1521,即立方體機器人系統(tǒng)有一個極點λ3在S平面的右半平面上,因此系統(tǒng)在平衡點處是不穩(wěn)定的。此外,式(19)的能控矩陣和能觀矩陣均為滿秩,即所設(shè)計的立方體機器人在坡面上既能控且能觀。因此,可以設(shè)計相應(yīng)的控制器,實現(xiàn)其坡面自平衡調(diào)節(jié)。

立方體機器人復(fù)雜的結(jié)構(gòu)和獨特的自平衡方式,使其難以獲取精確的動力學(xué)模型,導(dǎo)致采用常規(guī)的PID 控制方法存在控制精度不高、姿態(tài)不易收斂且抗干擾能力較差等問題。因此,本文將模糊邏輯與PID 相結(jié)合構(gòu)成立方體機器人的角速度-角度串級控制系統(tǒng),系統(tǒng)結(jié)構(gòu)如圖3 所示。

圖3 立方體機器人控制系統(tǒng)結(jié)構(gòu)圖

為了及時響應(yīng)立方體機器人機體角度的變化且克服不確定性擾動,選擇立方體機器人的機體角度θ作為副變量構(gòu)成基于PID 控制的副回路;為了彌補立方體機器人動力學(xué)模型不精確的問題和增強系統(tǒng)的魯棒性,以動量輪角速度為主變量并采用模糊PID 控制方式,構(gòu)成基于模糊PID的角速度-角度串級控制系統(tǒng),從而使得立方體機器人系統(tǒng)具有良好的控制品質(zhì)。

動量輪角速度輸入量偏差e及偏差變化率ec通過量化因子Ke、Kec進(jìn)行一次量化處理,輸出量Δkp、Δki、Δkd經(jīng)過解模糊化比例因子k1、k2、k3進(jìn)行一次比例變換。取動量輪角速度輸入量偏差e和偏差變化率ec,輸出量Δkp、Δki、Δkd模糊子集為{NB,NM,NS,Z,PS,PM,PB},分別代表負(fù)大,負(fù)中,負(fù)小,零,正小,正中,正大。論域為[-3,3],量化等級為{ -3,-2,-1,0,1,2,3}。輸入量偏差e和偏差變化率ec隸屬函數(shù)選為雙邊高斯型,輸出量Δkp、Δki、Δkd隸屬函數(shù)選為三角形,使用重心法進(jìn)行解模糊:

式中,ucen為面積中心對應(yīng)的橫坐標(biāo),A(ui) 為ui處的隸屬度,n為相關(guān)隸屬度的個數(shù)。系統(tǒng)的輸入輸出的函數(shù)隸屬度曲線如圖4、圖5 所示。

圖4 e、ec 隸屬函數(shù)

圖5 Δkp、Δki、Δkd 隸屬函數(shù)

根據(jù)動量輪角速度環(huán)kp、ki、kd3 個參數(shù)的作用、相互之間的互連關(guān)系及對輸出特性的影響,可以歸納出在不同e和ec的情況下,動量輪角速度環(huán)kp、ki、kd的自整定要求,從而可得模糊規(guī)則:

上述49 條模糊控制規(guī)則形成一個模糊控制規(guī)則矩陣,根據(jù)模糊輸入量決策出模糊輸出量

將Δkp、Δki、Δkd代入式(21),求出系統(tǒng)最終的控制參數(shù)kp、ki、kd。

其中,kp0、ki0、kd0為PID 初始值。

4 實驗與結(jié)果分析

4.1 對比仿真實驗

設(shè)立方體機器人的初始狀態(tài)為(5,0,0)T,本文所設(shè)計的基于模糊PID的立方體機器人串級控制系統(tǒng)和基于普通PID的立方體機器人控制系統(tǒng)的系統(tǒng)輸出響應(yīng)仿真曲線如圖6 所示。

由圖6 可知,相比于基于普通PID 控制的系統(tǒng),本文所設(shè)計的基于模糊PID的串級控制系統(tǒng)具有調(diào)節(jié)時間短、超調(diào)小、響應(yīng)速度快等特性。

圖6 模糊PID 串級控制與普通PID 仿真對比實驗圖

4.2 穩(wěn)定性實測實驗及結(jié)果分析

將立方體機器人放置在傾斜角度為23 °的坡面上,如圖7 所示,采樣周期為5 ms。穩(wěn)態(tài)時立方體機器人的機體角度跟隨和動量輪角速度實測輸出曲線如圖8 和圖9 所示。

圖7 立方體機器人實物實驗圖

圖8 立方體機器人穩(wěn)態(tài)時的機體角度跟隨曲線

圖9 立方體機器人穩(wěn)態(tài)時動量輪角速度輸出曲線

由圖8、圖9 可知,立方體機器人在坡面上穩(wěn)定時,動量輪的角速度在0 附近波動,機體角度值在設(shè)定值附近小幅度波動,立方體機器人具有良好的穩(wěn)定性。

4.3 抗擾性實測實驗及結(jié)果分析

立方體機器人在坡面上穩(wěn)態(tài)平衡過程中,給立方體機器人一個瞬間的干擾,使其機體角度值發(fā)生變化,立方體機器人系統(tǒng)的機體角度跟隨和動量輪角速度波形抗擾輸出響應(yīng)如圖10 和圖11 所示。

由圖10 和圖11 可知,對于立方體機器人坡面上穩(wěn)定過程中突然出現(xiàn)的外部干擾,立方體機器人系統(tǒng)能夠?qū)_動做出快速且有效的調(diào)整,保證立方體機器人的穩(wěn)定控制。

圖10 擾動情況下機體角度跟隨曲線

圖11 擾動情況下動量輪角速度輸出曲線

4.4 魯棒性實測實驗及結(jié)果分析

立方體機器人在坡面上穩(wěn)態(tài)平衡過程中,給立方體機器人的立方體框架上懸掛一個重物,改變立方體框架的質(zhì)量,系統(tǒng)的輸出響應(yīng)如圖12 和圖13所示。

圖13 不同系統(tǒng)參數(shù)下動量輪角速度輸出曲線

由圖12、圖13 可知,基于模糊PID的動量輪立方體機器人控制系統(tǒng),在系統(tǒng)參數(shù)發(fā)生變化時,仍然對立方體機器人系統(tǒng)的平衡控制具有良好的適應(yīng)性,驗證了本文控制算法對系統(tǒng)參數(shù)發(fā)生變化時有良好的魯棒性。

5 結(jié)論

本文自主研制了一種以立方體棱邊為支點的立方體機器人,針對其在坡面上自平衡動力學(xué)建模及控制問題,設(shè)計了基于模糊PID 控制的角速度-角度串級控制系統(tǒng)。首先,基于Lagrange 方程建立了立方體機器人坡面動力學(xué)模型并進(jìn)行了穩(wěn)定性和能控性、能觀性的理論分析。在此基礎(chǔ)上,選擇立方體機器人的機體角度為副變量、動量輪角速度為主變量,構(gòu)成基于模糊PID的立方體機器人串級控制系統(tǒng)。相比于常規(guī)的PID 單回路立方體機器人控制系統(tǒng),由于增加了角度副回路以及模糊邏輯的作用,坡面自平衡立方體機器人系統(tǒng)可以更好地克服由于對象非線性、各種擾動等不確定因素對系統(tǒng)的影響,具有良好穩(wěn)定性、抗干擾以及魯棒性。