單鈞麟，汪立新，秦偉偉，李 想

(火箭軍工程大學(xué) a.導(dǎo)彈工程學(xué)院; b.核工程學(xué)院， 西安 710025)

控制力矩陀螺(Control Moment Gyro,CMG)自平衡車是一類依靠CMG進(jìn)動(dòng)產(chǎn)生的陀螺力矩效應(yīng)保持姿態(tài)穩(wěn)定的新型無人駕駛裝置，當(dāng)其處于低速或靜止?fàn)顟B(tài)時(shí)由于自身靜不穩(wěn)定特性會(huì)出現(xiàn)傾倒現(xiàn)象，需要設(shè)計(jì)控制器實(shí)現(xiàn)其側(cè)傾穩(wěn)定控制[1-2]。同時(shí)在自平衡車穩(wěn)定控制過程中需要及時(shí)調(diào)整CMG進(jìn)動(dòng)角回到零位，防止其超出進(jìn)動(dòng)幅度造成系統(tǒng)失穩(wěn)。Lam等[3]采用雙閉環(huán)PD控制實(shí)現(xiàn)了傾角穩(wěn)定，但經(jīng)典輸入輸出反饋無法克服模型中的耦合項(xiàng)影響，未能有效控制進(jìn)動(dòng)角回零。趙明國(guó)等[4]采用極點(diǎn)配置方法設(shè)計(jì)狀態(tài)反饋控制器，在模型等效平臺(tái)上實(shí)現(xiàn)了穩(wěn)定及回零控制，但極點(diǎn)位置選擇對(duì)經(jīng)驗(yàn)依賴程度較大且無法保證良好的動(dòng)態(tài)性能。Sergio等[5]基于擴(kuò)張狀態(tài)觀測(cè)器設(shè)計(jì)了一種雙回路狀態(tài)反饋控制器并得到了較好的仿真控制效果，但控制器結(jié)構(gòu)復(fù)雜、能量消耗大且并未在實(shí)驗(yàn)平臺(tái)上對(duì)控制效果加以驗(yàn)證。

LQR控制器通過建立狀態(tài)變量和控制量構(gòu)成的目標(biāo)函數(shù)并求取最優(yōu)解實(shí)現(xiàn)狀態(tài)反饋控制，對(duì)于耦合系統(tǒng)的姿態(tài)穩(wěn)定控制具有較好的控制效果[6-7]。本文基于CMG自平衡車系統(tǒng)模型設(shè)計(jì)了一種粒子群算法優(yōu)化的LQR狀態(tài)反饋控制器，根據(jù)優(yōu)化流程通過Simulink仿真確定滿足系統(tǒng)性能指標(biāo)的控制器參數(shù)，并在實(shí)驗(yàn)平臺(tái)上實(shí)現(xiàn)了自平衡車實(shí)時(shí)側(cè)傾穩(wěn)定控制，同時(shí)能夠有效抑制進(jìn)動(dòng)角耦合和外加干擾力矩對(duì)系統(tǒng)的影響。

1 CMG自平衡車動(dòng)力學(xué)模型

1.1 系統(tǒng)組成及平衡原理

如圖1所示，CMG自平衡車采用單軌式前后兩輪結(jié)構(gòu)布局，車身后部安裝有CMG及其驅(qū)動(dòng)設(shè)備作為控制執(zhí)行機(jī)構(gòu)，中部是以NI my RIO控制板為核心器件的控制單元，底部固定IMU姿態(tài)測(cè)量單元，車身前部為電源供電裝置，最外側(cè)安裝有安全隔離架。

圖1 CMG自平衡車結(jié)構(gòu)

其中，CMG及其驅(qū)動(dòng)設(shè)備組成如圖2所示，包括飛輪轉(zhuǎn)子，自轉(zhuǎn)驅(qū)動(dòng)電機(jī)，陀螺框架，陀螺進(jìn)動(dòng)電機(jī)及增量式編碼器。

圖2 控制力矩陀螺組成

自轉(zhuǎn)電機(jī)驅(qū)動(dòng)飛輪轉(zhuǎn)子保持高速旋轉(zhuǎn)，當(dāng)自平衡車產(chǎn)生一定的傾斜角時(shí)，進(jìn)動(dòng)電機(jī)施加扭矩使飛輪轉(zhuǎn)子在自轉(zhuǎn)的同時(shí)相對(duì)車體進(jìn)動(dòng)，根據(jù)陀螺力矩效應(yīng)，陀螺進(jìn)動(dòng)過程中受到沿進(jìn)動(dòng)軸方向的外力矩同時(shí)會(huì)對(duì)車體產(chǎn)生一個(gè)反作用力矩抵消重力矩分量以及外界干擾力矩

M=H×ω

(1)

方向與飛輪自轉(zhuǎn)軸方向和進(jìn)動(dòng)軸方向均正交，如圖3所示。

圖3 陀螺力矩示意圖

1.2 模型建立

圖4 自平衡車模型及坐標(biāo)系

由上述假設(shè)和參數(shù)設(shè)置，可以得到自平衡車系統(tǒng)的動(dòng)能：

(2)

忽略不確定性外力作用，系統(tǒng)所受廣義力中重力為有勢(shì)力，則系統(tǒng)勢(shì)能為

V=mbgdbcosθ+mgfgdgfcosθ

(3)

選取車體的側(cè)傾運(yùn)動(dòng)和陀螺框架的進(jìn)動(dòng)兩個(gè)自由度為系統(tǒng)的廣義坐標(biāo)，根據(jù)包含有勢(shì)力的拉格朗日方程

對(duì)于廣義坐標(biāo)θ，受到的非有勢(shì)廣義力矩為

令q1=θ可得關(guān)于側(cè)傾角動(dòng)力學(xué)方程為

(4)

對(duì)于廣義坐標(biāo)α，受到的非有勢(shì)廣義力矩為

其中Km為進(jìn)動(dòng)電機(jī)的力矩放大系數(shù)，令q2=α可得進(jìn)動(dòng)角動(dòng)力學(xué)方程

(5)

2 側(cè)傾穩(wěn)定控制系統(tǒng)設(shè)計(jì)

2.1 系統(tǒng)性能分析

由方程式(4)、式(5)可知，CMG自平衡車模型中有非線性項(xiàng)和耦合項(xiàng)存在，直接基于非線性模型設(shè)計(jì)非線性控制器復(fù)雜度較高、實(shí)現(xiàn)困難，且實(shí)際穩(wěn)定控制范圍小并伴隨有抖動(dòng)現(xiàn)象[8]?？紤]自平衡車在實(shí)際應(yīng)用中偏離平衡位置范圍一般在-20°～20°，為降低控制器設(shè)計(jì)復(fù)雜度，對(duì)非線性模型在平衡點(diǎn)附近進(jìn)行線性化處理，可近似認(rèn)為有：

(6)

(7)

CMG自平衡車模型中涉及到的具體參數(shù)值如表1所示，將其代入狀態(tài)空間表達(dá)式中可以得到系統(tǒng)矩陣A和控制矩陣B分別為

求得系統(tǒng)極點(diǎn)[0,0,±0.860 6i]，可以發(fā)現(xiàn)系統(tǒng)有兩個(gè)零極點(diǎn)和兩個(gè)位于虛軸上的極點(diǎn)，因此該自平衡車系統(tǒng)是不穩(wěn)定的，但對(duì)系統(tǒng)可控性矩陣求秩得

rank(BABA2BA3B)=4

顯然可控性矩陣滿秩，系統(tǒng)可控并可設(shè)計(jì)狀態(tài)反饋控制器改變系統(tǒng)極點(diǎn)使其達(dá)到穩(wěn)定狀態(tài)。

表1 自平衡車參數(shù)

2.2 粒子群優(yōu)化LQR控制器設(shè)計(jì)

對(duì)于CMG自平衡車的側(cè)傾穩(wěn)定控制，主要考慮系統(tǒng)是否能快速、平穩(wěn)地達(dá)到平衡狀態(tài)，以及解決平衡車傾角和CMG進(jìn)動(dòng)角控制耦合問題。同時(shí)采用CMG作為控制執(zhí)行機(jī)構(gòu)雖然具有力矩放大顯著、響應(yīng)快等優(yōu)點(diǎn)，但其耗費(fèi)能量較大，因此設(shè)計(jì)以狀態(tài)變量θ、α和控制量u為性能指標(biāo)的線性定常系統(tǒng)無限時(shí)間狀態(tài)調(diào)節(jié)控制器：

其中，對(duì)于狀態(tài)約束矩陣Q和控制約束矩陣R的選取并沒有明確指標(biāo)，較難通過解析方式得到，本文采用粒子群優(yōu)化算法結(jié)合Simulink仿真迭代的方式進(jìn)行求解[9]，各權(quán)重系數(shù)變換相同倍數(shù)并不影響相互之間比例關(guān)系，不妨假設(shè)Q、R矩陣具有如下形式：

粒子群優(yōu)化算法采用帶慣性權(quán)重的速度和位置更新式[10]：

(7)

適應(yīng)度函數(shù)選擇能夠反映控制系統(tǒng)穩(wěn)定性和快速性的ITAE誤差指標(biāo)[11]：

(8)

LQR控制器權(quán)重系數(shù)優(yōu)化流程和Simulink仿真模型分別如圖5、圖6所示。

圖5 LQR權(quán)重系數(shù)優(yōu)化流程框圖

圖6 Simulink仿真模型示意圖

3 實(shí)驗(yàn)驗(yàn)證

搭建自平衡車實(shí)驗(yàn)平臺(tái)如圖7，NI myRIO控制板集成了雙核 ARM Cortex-A9實(shí)時(shí)處理器以及Xilinx FPGA可編程器件，基于Real-Time模塊能夠編譯并執(zhí)行LabVIEW實(shí)時(shí)控制程序，并通過Xsens三軸IMU和增量式編碼器NI-CompactRio-9114實(shí)時(shí)敏感車體姿態(tài)和控制力矩陀螺進(jìn)動(dòng)角信息。實(shí)驗(yàn)中設(shè)定控制力矩陀螺進(jìn)動(dòng)幅度±45°，采樣頻率100 Hz，LQR控制器參數(shù)經(jīng)粒子群算法優(yōu)化后，最優(yōu)適應(yīng)度函數(shù)值f=2.058，權(quán)重系數(shù)q1=150.32,q3=501.85，結(jié)合Riccati方程可以計(jì)算得到反饋控制矩陣K=[-1 328.3-223.6-15.818.8]。

圖7 實(shí)驗(yàn)驗(yàn)證平臺(tái)

1) 具有初始傾角的姿態(tài)穩(wěn)定控制。分別設(shè)定平衡車傾角初始值為6°、8°、10°，姿態(tài)穩(wěn)定控制曲線如圖8所示，可以看出在平衡范圍內(nèi)隨著初始傾角增大，系統(tǒng)仍能保持較好地控制效果，在1.7 s左右回到平衡狀態(tài)，超調(diào)角度均在2°左右，調(diào)節(jié)過程平穩(wěn)。傾角在平衡點(diǎn)附近穩(wěn)態(tài)誤差為0.25°，證明粒子群優(yōu)化LQR控制器具有較高的控制精度。

圖8 具有初始傾角的姿態(tài)穩(wěn)定控制曲線

2) CMG進(jìn)動(dòng)回零控制。自平衡車側(cè)傾穩(wěn)定控制過程中伴隨有CMG進(jìn)動(dòng)作用產(chǎn)生，平衡車姿態(tài)回到平衡位置后CMG進(jìn)動(dòng)角仍然存在，當(dāng)進(jìn)動(dòng)角達(dá)到限定幅度時(shí)，CMG無法由進(jìn)動(dòng)效應(yīng)產(chǎn)生陀螺力矩保持系統(tǒng)姿態(tài)穩(wěn)定，為了提升系統(tǒng)魯棒性能設(shè)定進(jìn)動(dòng)角零位期望值，控制曲線如圖9所示，可以看出所設(shè)計(jì)的控制器受耦合影響較小，能夠在實(shí)現(xiàn)側(cè)傾穩(wěn)定控制的同時(shí)(2 s左右)快速、平穩(wěn)控制進(jìn)動(dòng)角回到零位。

圖9 CMG進(jìn)動(dòng)角回零控制曲線

3) 外加力矩干擾姿態(tài)穩(wěn)定控制。為了進(jìn)一步驗(yàn)證控制器對(duì)自平衡車姿態(tài)穩(wěn)定控制的抗干擾能力，在t=3 s時(shí)對(duì)處于平衡狀態(tài)的平衡車施加脈沖力矩干擾，實(shí)驗(yàn)結(jié)果如圖10所示，經(jīng)過2 s左右傾角恢復(fù)平衡狀態(tài)，證明系統(tǒng)在外界環(huán)境突變情況下仍能保持姿態(tài)穩(wěn)定，具有一定的抗干擾能力。

圖10 外加干擾姿態(tài)穩(wěn)定實(shí)驗(yàn)結(jié)果

4 結(jié)論

設(shè)計(jì)了一種粒子群算法優(yōu)化的控制力矩陀螺自平衡車LQR控制器；將LQR控制器應(yīng)用于控制力矩陀螺自平衡車側(cè)傾穩(wěn)定控制，基于粒子群算法設(shè)計(jì)控制器Q、R權(quán)重系數(shù)矩陣優(yōu)化流程，結(jié)合Simulink仿真確定控制器參數(shù)值并計(jì)算得到最優(yōu)狀態(tài)反饋控制律，最后在搭建的實(shí)驗(yàn)平臺(tái)上進(jìn)行實(shí)時(shí)控制實(shí)驗(yàn)，結(jié)果表明本文所設(shè)計(jì)的控制器能夠快速、平穩(wěn)實(shí)現(xiàn)自平衡車側(cè)傾穩(wěn)定控制，穩(wěn)態(tài)誤差小，同時(shí)能夠有效調(diào)節(jié)CMG進(jìn)動(dòng)角回到零位，使系統(tǒng)具有抗干擾能力。