基于LQR對(duì)直線倒立擺的穩(wěn)擺控制研究及實(shí)現(xiàn)

張慧慧，侯伯杰，高建設(shè)，郭 明

（鄭州大學(xué)機(jī)械與動(dòng)力工程學(xué)院，河南 鄭州 450001）

1 引言

隨著計(jì)算機(jī)的快速發(fā)展，最優(yōu)控制在工程上的推廣與應(yīng)用越來越廣泛，并且仍是活躍的研究領(lǐng)域［1］。其中最優(yōu)控制策略中LQR（Linear Quadratic Regulator）即線性二次型調(diào)節(jié)器是應(yīng)用較為廣泛的最優(yōu)控制方法。其根據(jù)研究對(duì)象的線性狀態(tài)空間方程，求解最優(yōu)反饋控制率，使選定的二次型性能指標(biāo)達(dá)到極值。LQR控制性能與所選的二次型性能指標(biāo)中的加權(quán)矩陣Q、R的選擇密切相關(guān)，但是它們之間尚未形成明確的對(duì)應(yīng)關(guān)系［2］。倒立擺系統(tǒng)是一種多變量、非線性、不穩(wěn)定的耦合欠驅(qū)動(dòng)機(jī)械系統(tǒng)［3］，被廣泛地應(yīng)用于控制理論的教學(xué)、實(shí)驗(yàn)和驗(yàn)證控制算法的有效性［4］。倒立擺的穩(wěn)擺控制方法在軍工、航天、機(jī)器人領(lǐng)域和工業(yè)工程上具有很廣泛的用途［5］，如人型機(jī)器人行走過程中的平衡控制、火箭發(fā)射中的垂直度控制和衛(wèi)星的姿態(tài)控制等這些類似于倒立擺的重心在上，支點(diǎn)在下的倒置控制問題［6-7］。因此，研究LQR控制器對(duì)倒立擺的穩(wěn)擺控制不僅有深遠(yuǎn)的理論研究意義，而且在工程應(yīng)用上有一定的指導(dǎo)意義。

目前，國內(nèi)外有許多有關(guān)倒立擺的LQR控制器研究的文章，但其中很多文章只局限于理論分析和仿真驗(yàn)證，而缺少在實(shí)物倒立擺系統(tǒng)上進(jìn)行的實(shí)驗(yàn)驗(yàn)證。如文獻(xiàn)［8］研究環(huán)形倒立擺的LQR控制器，利用改進(jìn)遺傳算法對(duì)LQR的Q和R進(jìn)行優(yōu)化選擇，其最后只通過仿真證明采用改進(jìn)遺傳算法的LQR控制器可有效的提高了環(huán)形倒立擺系統(tǒng)的穩(wěn)定性；文獻(xiàn)［9］研究二級(jí)直線倒立擺系統(tǒng)的LQR 控制器，通過采用Simulink仿真驗(yàn)證LQR 控制器能夠?qū)崿F(xiàn)對(duì)二級(jí)直線倒立擺的控制；文獻(xiàn)［10］研究了直線倒立擺的LQR控制器，采用試湊法確定Q和R，但是最后只通過仿真證明其設(shè)計(jì)的LQR控制器的有效性。

這里研究了直線倒立擺的LQR 控制器設(shè)計(jì)過程，并設(shè)計(jì)了倒立擺穩(wěn)擺控制的LQR控制器設(shè)計(jì)；為了驗(yàn)證設(shè)計(jì)的LQR控制器，創(chuàng)新設(shè)計(jì)了一種以計(jì)算機(jī)+開放式運(yùn)動(dòng)控制器+交流伺服電機(jī)為控制核心的倒立擺系統(tǒng)平臺(tái)，用于驗(yàn)證LQR對(duì)直線倒立擺系統(tǒng)穩(wěn)擺控制。此實(shí)驗(yàn)平臺(tái)具有編程簡單、開放性高，為研究直線倒立擺的穩(wěn)擺控制以及驗(yàn)證倒立擺的控制算法提供一種新的研究思路，為后續(xù)的研究打下了基礎(chǔ)。

2 倒立擺數(shù)學(xué)模型

為使建立的倒立擺模型與實(shí)際系統(tǒng)更加符合，不忽略小車滑塊與導(dǎo)軌間的滑動(dòng)摩擦和擺桿旋轉(zhuǎn)時(shí)的轉(zhuǎn)動(dòng)副摩擦阻力矩。對(duì)倒立擺系統(tǒng)的模型做一定的假設(shè)如下：小車為質(zhì)點(diǎn)，其受到的摩擦力與小車的速度成正比；擺桿為質(zhì)量均勻的剛體，其轉(zhuǎn)動(dòng)的阻力矩與角速度成正比；忽略空氣阻力。一級(jí)直線倒立擺系統(tǒng)簡化模型與受力分析，如圖1所示。

圖1 倒立擺系統(tǒng)簡化模型與受力分析Fig.1 Simplified Model and Force Analysis of Inverted Pendulum System

倒立擺系統(tǒng)簡化模型圖，如圖1（a）所示。倒立擺系統(tǒng)受力分析圖，如圖1（b）所示。在圖1中，F(xiàn)—施加在小車上的驅(qū)動(dòng)力；x—小車的位移；M—小車的質(zhì)量；m—擺桿質(zhì)量；b—小車移動(dòng)的摩擦系數(shù)；I—擺桿繞其質(zhì)心的轉(zhuǎn)動(dòng)慣量（）；η—擺桿轉(zhuǎn)動(dòng)阻力矩系數(shù)；θ—擺桿與豎直向下方向的夾角；?—擺桿與豎直向上方向的夾角（逆時(shí)針為正）。依據(jù)牛頓力學(xué)與運(yùn)動(dòng)定理，推導(dǎo)直線倒立擺系統(tǒng)的動(dòng)力學(xué)方程的過程如下：

分析小車在水平方向上受力平衡，可得：

分析擺桿在水平方向上受力平衡，可得：

式（1）、式（2）中的N—滑塊與擺桿水平方向間的相互作用力，聯(lián)立式（1）、式（2）兩個(gè)方程消除N，得到一級(jí)直線倒立擺的第一個(gè)非線動(dòng)力學(xué)模型方程：

分析擺桿在豎直方向上受力平衡，可得：

分析擺桿繞其質(zhì)心的力矩平衡，可得：

式（2）、式（4）、式（5）方程中的N、P—滑塊與擺桿間水平、豎直方向間的相互作用力，聯(lián)立式（2）、式（4）、式（5）三個(gè)方程消除P、N，得到一級(jí)直線倒立擺第二個(gè)非線動(dòng)力學(xué)模型方程：

倒立擺穩(wěn)擺控制是在-0.3rad

因此，可對(duì)倒立擺系統(tǒng)非線性動(dòng)力學(xué)方程式（3）與式（6）進(jìn)行線性化處理，得到一級(jí)直線倒立擺系統(tǒng)線性化動(dòng)力學(xué)方程：

記狀態(tài)向量X=[]T與u=F，根據(jù)方程組（7）可得到一級(jí)直線倒立擺系統(tǒng)的狀態(tài)空間方程如下：

對(duì)自主設(shè)計(jì)的倒立擺機(jī)械結(jié)構(gòu)進(jìn)測量，得到系統(tǒng)的物理參數(shù)及物理意義，如表1所示。

表1 倒立擺系統(tǒng)物理參數(shù)表Tab.1 Physical Parameters of Inverted Pendulum System

把表1中倒立擺系統(tǒng)物理參數(shù)代入式（9）、式（10），得到倒立擺系統(tǒng)矩陣A，輸入矩陣B如下：

對(duì)倒立擺系統(tǒng)的穩(wěn)定性分析：開環(huán)系統(tǒng)的特征方程為：

根據(jù)式（15）計(jì)算倒立擺開環(huán)系統(tǒng)的特征值為：

由式（16）可知倒立擺開環(huán)系統(tǒng)，S平面的右側(cè)有兩個(gè)開環(huán)極點(diǎn)，因此系統(tǒng)不穩(wěn)定。

計(jì)算得rand(To)=4，所以系統(tǒng)完全可控。

計(jì)算得rand(Co)=4，所以系統(tǒng)完全可觀。

3 LQR控制器設(shè)計(jì)

LQR控制器的基本原理是：對(duì)于可控的線性定常系統(tǒng)，其狀態(tài)空間方程的形式為：

式中：A—系統(tǒng)矩陣；B—輸入矩陣；C—輸出矩陣；X—狀態(tài)向量；u—控制率；y—輸出向量。設(shè)控制率u不受約束尋找最優(yōu)，使式（20）性能指標(biāo)最小。

式中：Q—半正定對(duì)稱常數(shù)的狀態(tài)向量加權(quán)矩陣；R—正定對(duì)稱常數(shù)的控制率加權(quán)矩陣。

通過求解黎卡提代數(shù)方程：

可以得到P與最優(yōu)狀態(tài)反饋增益矩陣K的值，使性能指標(biāo)J最小的控制率u，控制率u為：

因此，LQR控制的控制性能與性能指標(biāo)中的加權(quán)矩陣Q、R的選擇密切相關(guān)，合理的加權(quán)陣可使系統(tǒng)響應(yīng)快速、平穩(wěn)，反之無法滿足設(shè)計(jì)要求。

對(duì)于LQR 控制器的設(shè)計(jì)，在選取加權(quán)矩陣Q、R后，在MATLAB中調(diào)用lqr函數(shù)，即可得到系統(tǒng)的狀態(tài)反饋增益矩陣K=lq（rA，B，Q，R），完成LQR控制器設(shè)計(jì)。而加權(quán)矩陣Q、R的選擇與控制性能之間尚未形成明確的對(duì)應(yīng)關(guān)系，下面采用仿真試湊確定Q、R，設(shè)計(jì)倒立擺系統(tǒng)的LQR控制器。

倒立擺系統(tǒng)控制器設(shè)計(jì)要求：對(duì)于給定的擺桿初始擺角?=0.3rad 和初始擺速與小車初始位移x=0m 和初始速度，設(shè)計(jì)一個(gè)使擺桿保持在倒立垂直狀態(tài)控制率u，同時(shí)要求在控制過程結(jié)束后，小車返回到期望位置x=0m。指標(biāo)要求為：系統(tǒng)恢復(fù)穩(wěn)定的調(diào)整時(shí)間不超過2.5s；小車調(diào)整的位移幅度小于0.25m；擺桿擺角與小車位移調(diào)整的峰值時(shí)間小于0.5s。

由建立的一級(jí)直線倒立擺系統(tǒng)狀態(tài)空間方程和對(duì)系統(tǒng)分析可知：其開環(huán)系統(tǒng)是不穩(wěn)定的、可控的與可觀的；其系統(tǒng)有四個(gè)狀態(tài)量分別是小車位移、小車速度、擺桿擺角和擺桿角速度，狀態(tài)向量X=[]T，系統(tǒng)為單輸入單控制率。

因此，一級(jí)直線倒立擺系統(tǒng)的LQR控制器性能指標(biāo)中，加權(quán)矩陣Q=diag {q11q22q33q44}，R—單元素矩陣。

對(duì)加權(quán)矩陣Q和R的選擇，最簡單的情況是R=1 和Q=CTC=diag{1 0 1 0}，此選擇認(rèn)定控制率u與狀態(tài)量小車位移和擺桿擺角在LQR的性能指標(biāo)中同等重要。按R和Q最簡單情況的選擇，在MATLAB里調(diào)用lqr函數(shù)，計(jì)算得到狀態(tài)反饋增益矩陣K=[-1-1.5273 15.7149 2.3523]。在Matlab 中仿真，初始條件為X(0)=[0 0 0.3 0]T的系統(tǒng)仿真響應(yīng)曲線，如圖2所示。

圖2 系統(tǒng)仿真響應(yīng)曲線Fig.2 System Simulation Response Curve

從圖2可以看出：擺桿與小車經(jīng)過約7s的調(diào)節(jié)系統(tǒng)恢復(fù)穩(wěn)定狀態(tài)，小車的超調(diào)位移-0.45m，超調(diào)峰值時(shí)間1s，擺桿超調(diào)峰值時(shí)間大于0.5s。可見選擇的這組加權(quán)矩陣Q和R設(shè)計(jì)的LQR控制器不滿足要求。

經(jīng)過試湊仿真，當(dāng)加權(quán)矩陣Q=diag{400 10 20 10}和R=1，狀態(tài)反饋增益K=[-20-13.8971 49.9676 8.3332]。在matlab中仿真，初始條件為X(0)=[0 0 0.3 0]T的系統(tǒng)仿真響應(yīng)曲線，如圖3所示。

圖3 Q、R試湊后的系統(tǒng)仿真響應(yīng)曲線Fig.3 System Simulation Response Curve After Q and R Trial

從圖3可以看出：擺桿與小車經(jīng)過約2s的調(diào)節(jié)系統(tǒng)恢復(fù)穩(wěn)定狀態(tài)，小車的超調(diào)位移-0.2m，超調(diào)峰值時(shí)間小于0.5s，擺桿超調(diào)峰值時(shí)間小于0.5s，滿足設(shè)計(jì)要求完成控制器設(shè)計(jì)。

4 倒立擺系統(tǒng)軟硬件結(jié)構(gòu)

直線倒立擺系統(tǒng)是由可以沿直線導(dǎo)軌運(yùn)動(dòng)的小車與一端通過轉(zhuǎn)動(dòng)副固定到小車上的均勻細(xì)桿組成的系統(tǒng)，采用SolidWorks設(shè)計(jì)的倒立擺系統(tǒng)其結(jié)構(gòu)示意圖，如圖4所示。搭建完成的倒立擺系統(tǒng)實(shí)物，如圖5所示。擺桿采用304不銹鋼；滑塊為輕質(zhì)鋁材內(nèi)有直線軸承；以交流伺服電機(jī)作為驅(qū)動(dòng)，同步帶作為傳動(dòng)結(jié)構(gòu)，帶動(dòng)滑塊在光軸上直線運(yùn)動(dòng)；絕對(duì)值編碼器作為轉(zhuǎn)動(dòng)副連接小車與擺桿以及用來采集擺桿的擺角；底座采用鋁型材搭建，并且在平臺(tái)導(dǎo)軌兩端分別安裝了限位開關(guān)，防止小車滑塊移動(dòng)過量損壞平臺(tái)。

圖4 倒立擺系統(tǒng)結(jié)構(gòu)示意圖Fig.4 Schematic Diagram of the Structure of the Inverted Pendulum System

圖5 倒立擺系統(tǒng)實(shí)物圖Fig.5 Physical Picture of the Inverted Pendulum System

倒立擺控制系統(tǒng)采用以計(jì)算機(jī)+開放式運(yùn)動(dòng)控制器+交流伺服電機(jī)為控制核心，倒立擺控制系統(tǒng)結(jié)構(gòu)，如圖6所示。開放式運(yùn)動(dòng)控制器采用正運(yùn)動(dòng)公司生產(chǎn)的開放式運(yùn)動(dòng)控制器ZMC432，支持EtherCAT總線通訊，ECAT最快1ms的刷新周期。執(zhí)行機(jī)構(gòu)采用臺(tái)達(dá)公司生產(chǎn)的100w 交流伺服電機(jī)。擺角傳感器采用15位分辨率、RS485接口標(biāo)準(zhǔn)ModbusRTU 輸出協(xié)議的絕對(duì)值數(shù)字編碼器，可避免擺角信號(hào)的零點(diǎn)漂移、噪聲干擾等缺點(diǎn)。忽略同步帶的形變，通過交流伺服電機(jī)內(nèi)置的光電編碼器，可實(shí)時(shí)獲取小車位移與速度。

圖6 倒立擺控制系統(tǒng)結(jié)構(gòu)Fig.6 Inverted Pendulum Control System Structure

在開放式運(yùn)動(dòng)控制器ZMC432里編寫倒立擺系統(tǒng)穩(wěn)擺控制的LQR最優(yōu)控制策略算法，控制系統(tǒng)程序流程，如圖7所示。倒立擺控制系統(tǒng)程序采用雙線程，主線程用于運(yùn)動(dòng)控制器與計(jì)算機(jī)信息交換，輔線程里編寫倒立擺LQR控制器算法程序。主線程的程序流程：首先對(duì)全局變量、通信協(xié)議初始化，再對(duì)伺服電機(jī)驅(qū)動(dòng)器初始化，之后進(jìn)入主循環(huán)。主循環(huán)里循環(huán)掃描電機(jī)報(bào)警異常，循環(huán)向計(jì)算機(jī)端發(fā)送擺角與位移等信息數(shù)據(jù)，循環(huán)接收計(jì)算機(jī)人發(fā)送回的LQR反饋增益矩陣參數(shù)與輔線程啟停指令。

圖7 倒立擺控制系統(tǒng)程序流程Fig.7 Inverted Pendulum Control System Program Flow

主線程里收到輔線程啟停指令，控制輔線程的啟停，實(shí)現(xiàn)倒立擺LQR控制器程序的啟?？刂?。

為基于C#編程開發(fā)計(jì)算機(jī)端實(shí)時(shí)人機(jī)交互軟件，如圖8 所示。計(jì)算機(jī)與運(yùn)動(dòng)控制器之間通過EtherNET連接進(jìn)行數(shù)據(jù)交換通信。

圖8 計(jì)算機(jī)人機(jī)交互界面Fig.8 Computer Man-Machine Interface

在計(jì)算機(jī)端實(shí)現(xiàn)對(duì)倒立擺系統(tǒng)的擺角、位置等關(guān)鍵數(shù)據(jù)的實(shí)時(shí)顯示以及記錄。通過鍵盤可以方便得調(diào)整LQR控制器反饋增益矩陣參數(shù)，設(shè)置小車的期望位移，及修正擺桿豎直倒立時(shí)編碼器的零值等功能。

5 實(shí)驗(yàn)結(jié)果及對(duì)比分析

采用LQR 調(diào)節(jié)器，根據(jù)設(shè)計(jì)結(jié)果設(shè)置一級(jí)直線倒立擺控制系統(tǒng)參數(shù)。倒立擺系統(tǒng)的跟蹤仿真響應(yīng)曲線，如圖9所示。倒立擺系統(tǒng)的實(shí)測試響應(yīng)曲線，如圖10所示。

圖9 倒立擺系統(tǒng)的仿真跟蹤響應(yīng)曲線Fig.9 Simulation Tracking Response Curve of Inverted Pendulum System

圖10 倒立擺系統(tǒng)的實(shí)測跟蹤響應(yīng)曲線Fig.10 Measured Tracking Response Curve of Inverted Pendulum System

從圖9可以看出：在t=0s時(shí)刻，施加一個(gè)位移上的階躍信號(hào)，即是把小車的期望位移由0調(diào)整為0.1m，在控制器的調(diào)節(jié)下，經(jīng)過大約1.5s 小車運(yùn)動(dòng)到期望位移0.1m 處，擺桿保持在倒立平穩(wěn)狀態(tài)。

從圖10可以看出：在t=1.5s前，系統(tǒng)在控制器的作用下，擺桿可以保持在倒立平穩(wěn)狀態(tài)，小車保持在x=0處，在t=1.5s時(shí)，施加一個(gè)位移上的階躍信號(hào)時(shí)，即把小車的期望位由0調(diào)整為0.1m，在控制器的調(diào)節(jié)下，經(jīng)過大約1.5s小車運(yùn)動(dòng)到期望位移0.1m處，擺桿保持在倒立平穩(wěn)狀態(tài)。

對(duì)比圖9、圖10可以看出：設(shè)計(jì)的LQR控制器對(duì)直線倒立擺穩(wěn)擺控制是有效的和可行的。

但實(shí)際控制與仿真也有一定的不同，實(shí)測響應(yīng)曲線顯示擺桿與小車很難在平衡點(diǎn)不動(dòng)，而是在平衡點(diǎn)附近振動(dòng)，小車位移的振動(dòng)幅度小于±0.01m，擺桿擺角的振動(dòng)幅度小于±0.05rad。

造成這種不同的原因是由于同步帶的伸縮、震動(dòng)、采樣延時(shí)以及控制率的輸出延伸，致使擺桿與小車很難在平衡點(diǎn)不動(dòng)而是在平衡點(diǎn)附近振動(dòng)。

6 結(jié)論

由上述實(shí)驗(yàn)結(jié)果可知這里所設(shè)計(jì)的LQR控制器對(duì)一級(jí)直線倒立擺穩(wěn)擺控制是有效的和可行的。

采用仿真試湊法選擇的加權(quán)矩陣Q、R是合理的，LQR設(shè)計(jì)過程是正確的。

設(shè)計(jì)的以計(jì)算機(jī)+開放式運(yùn)動(dòng)控制器+交流伺服電機(jī)為核心的直線倒立擺系統(tǒng)平臺(tái)，具有操作簡單、開放性高的優(yōu)點(diǎn)，可以用來研究直線倒立擺的穩(wěn)擺控制以及驗(yàn)證倒立擺的控制算法。

設(shè)計(jì)的倒立擺測試平臺(tái)對(duì)小車位移x與擺桿擺角?測量采用的是數(shù)字編碼器，它在數(shù)據(jù)傳輸、處理過程中全部采用了數(shù)字處理，避免了采樣的零點(diǎn)漂移、噪聲等干擾。

但是由于同步帶的伸縮、震動(dòng)與采樣延時(shí)，使擺桿與小車很難在平衡點(diǎn)不動(dòng)而是在平衡點(diǎn)附近振動(dòng)，造成仿真與實(shí)測實(shí)驗(yàn)有一定的不同。