基于MPC的二階倒立擺穩(wěn)定控制

石轉(zhuǎn)轉(zhuǎn)，郭開璽，張 品，張占東

（山西大同大學 機電工程學院，大同 037003）

倒立擺系統(tǒng)是一個不穩(wěn)定、多變量、非線性的機械系統(tǒng)，在控制過程中，倒立擺系統(tǒng)可以有效地反映出控制系統(tǒng)的非線性、魯棒性等問題，因此，在控制領(lǐng)域中經(jīng)常使用倒立擺來檢驗新的控制策略和算法的有效性[1-2]。 倒立擺為機器人系統(tǒng)中的大量技術(shù)應用提供了足夠的模型[3]，它們被用作許多實際控制問題的代表性模型： 比如人的手臂運動、火箭的發(fā)射[4]、機器人等。

二階倒立擺為一階倒立擺的擴展版本，因為它在一階倒立擺系統(tǒng)中增加了一個擺。 因此，與一階倒立擺相比，二階倒立擺是一個高階、絕對非線性、不穩(wěn)定的系統(tǒng)[5]。 這使得二階倒立擺的控制和穩(wěn)定成為一個非常具有挑戰(zhàn)性的問題。 研究人員為此提出了不同的控制技術(shù)來控制二階倒立擺系統(tǒng)。 模型預測控制技術(shù)是一種處理多變量約束系統(tǒng)最優(yōu)控制問題的最有效方法之一， 與其他控制算法相比，模型預測控制（MPC）提供了一種使輸出誤差和控制工作量最小化的最優(yōu)控制方案。MPC 使用系統(tǒng)的狀態(tài)空間模型預測未來輸出，并將預測的輸出誤差合并到二次成本函數(shù)中。 在控制過程中，利用二次規(guī)劃方法使成本函數(shù)最小化，并使其服從系統(tǒng)的硬約束。 因此，MPC 可以有效地處理系統(tǒng)的約束[6-7]。

本文提出使用模型預測控制對二階倒立擺系統(tǒng)進行穩(wěn)定控制，在Python 中建立倒立擺拉格朗日系統(tǒng)模型， 同時設(shè)計模型預測控制快速預測控制器，對處于不同情況下二階倒立擺系統(tǒng)的穩(wěn)定控制做了數(shù)值仿真計算分析。

1 理論

1.1 系統(tǒng)數(shù)學模型

二階倒立擺系統(tǒng)是一種典型的欠驅(qū)動機械系統(tǒng)，其主要由小車和兩根擺桿組成，兩個擺桿連接到水平面自由移動的小車上。 它有3 個自由變量，即拉力F（水平位置x），第一擺角θ1和第二擺角θ2，二階倒立擺系統(tǒng)示意圖如圖1 所示。

圖1 機械式二階倒立擺系統(tǒng)Fig.1 Mechanical double inverted pendulum system

其中F 是控制輸入，表示水平拉力；θ1∈（-π，π）表示擺桿1（即下擺桿，鏈接在小車上）與豎直垂線之間的夾角；θ2∈（-π，π）表示擺桿2（即上擺桿，鏈接在擺桿1 的另一端）與豎直垂線之間的夾角。 此外；m，m1和m2分別代表小車、擺桿1 和擺桿2 的質(zhì)量；L1和L2分別代表擺桿1 和擺桿2 的長度；l1和l2分別表示擺桿1 和擺桿2 擺桿質(zhì)心到低端的長度；J1和J2分別表示擺桿1 和擺桿2 的轉(zhuǎn)動慣量。此外，二階倒立擺系統(tǒng)的機械參數(shù)取值如表1 所示，g 表示重力加速度，參數(shù)取自于文獻[8]。

表1 二階倒立擺系統(tǒng)仿真參數(shù)Tab.1 Simulation parameters of the double-inverted-pendulum system

式中：z 為廣義坐標；La 為拉格朗日函數(shù)；T，V 和Fi分別代表系統(tǒng)總的動能、勢能和沿廣義坐標方向上的力。 選取x，θ1和θ2作為廣義坐標。 基于公式La=T-V，可以得出：

二階倒立擺系統(tǒng)總動能為T=Tm+Tm1+Tm2， 其中Tm為小車的動能，Tm1和Tm2分別為擺桿1 和擺桿2的動能。 小車和兩根擺桿的動能分別為

則系統(tǒng)總動能T 為

二階倒立擺系統(tǒng)的總勢能為V=Vm+Vm1+Vm2，其中Vm為小車的勢能，Vm1和Vm2分別為擺桿1 和擺桿2 的勢能。

小車和兩根擺桿的勢能分別為

式中：y1=l1cosθ1，y2=L1cosθ1+l2cosθ2則系統(tǒng)的總勢能V 為

將式（4）和式（6）代入式（2）以獲得拉格朗日算子：

最后，將公式（7）帶入到公式（2），可以得到直線二階倒立擺系統(tǒng)的數(shù)學模型，其形式如下：

其中：

1.2 模型線性化

上述方程（8）描述了系統(tǒng)的運動，顯然是非線性的，因此，需要將其線性化。 選取系統(tǒng)的狀態(tài)向量控制輸入量u=F，輸出變量為最后可得到倒立擺系統(tǒng)的線性狀態(tài)空間方程為

式中：I 為單位矩陣，系數(shù)矩陣A，B，C 分別為

2 模型預測控制

MPC 是一種反饋控制方法，使用狀態(tài)變量的當前狀態(tài)值和目標值來預測并提前優(yōu)化系統(tǒng)響應。 目標是使系統(tǒng)響應接近目標值。 通常情況下，MPC 會優(yōu)化計算機上的一系列狀態(tài)軌跡通過提供最佳輸入來減少成本[9-10]。它使用當前狀態(tài)變量來尋找一系列最佳輸入，在下一個采樣周期更新狀態(tài)變量，以及在下一時間步中重復計算。 以此得到一系列控制序列，控制序列將按照一個特定時間間隔進行計算，稱為控制時域，同時，它將在較長的時間間隔內(nèi)確定系統(tǒng)的進度，稱為預測時域[11]。 MPC 的基本原理如圖2 所示。

圖2 MPC 基本原理圖Fig.2 Basic schematic diagram of MPC

模型預測控制解決問題的第一步就是模型的離散化， 隨后將在每個采樣時刻求解離散化問題，通常使用以下形式的離散模型：

式中：x（k）和u（k）分別代表系統(tǒng)當前狀態(tài)和控制輸入，根據(jù)當前狀態(tài)x（k）和輸入u（k）映射到下一個狀態(tài)x（k+1），使系統(tǒng)預測得到狀態(tài)在預測時域上的軌跡[12]，通過離散化狀態(tài)方程依次對未來整個控制周期的系統(tǒng)狀態(tài)進行預測，可以得到：

顯然，系統(tǒng)在未來某個時刻K 的狀態(tài)，取決于初始狀態(tài)x（1）和輸入u（k）。當對動態(tài)系統(tǒng)未來狀態(tài)進行預測時，需求得整個預測時域內(nèi)的控制輸入量U，即為后續(xù)將要求解的優(yōu)化問題的獨立變量：

在每個采樣時刻都將進行優(yōu)化， 尋找最優(yōu)解，通過最小化決定控制性能的成本函數(shù)來實現(xiàn)，成本函數(shù)定義為

式中：Q 是半正定狀態(tài)加權(quán)矩陣，用于約束系統(tǒng)狀態(tài)偏差；R 是輸入加權(quán)矩陣，是指約束施加給被控對象的控制量u 的大小。

由于在倒立擺系統(tǒng)運行過程中，對于小車和雙擺穩(wěn)定性，動力性等方面的考慮，需要添加對控制輸入量力的約束：

式中：xub和xlb分別為狀態(tài)的上限和下限，uub和ulb分別輸入量拉力的最大值和最小值。 通過在未來每個采樣時刻的控制動作中最小化J 來確定U，得到一系列控制序列：

將第一個控制動作應用于系統(tǒng)，作為本控制周期的控制量，下一次根據(jù)新的樣本重復該過程[13]。

3 仿真研究

為了評估MPC 控制方法的控制性能，本文通過利用Python 編程對二階倒立擺進行了數(shù)值仿真研究，忽略空氣阻力和各種摩擦，將二階倒立擺系統(tǒng)抽象成小車和均勻質(zhì)桿組成的系統(tǒng)并建立直角坐標系，如圖3 所示。 控制目標是小車在初始位置附近運動時， 雙擺能夠精確的穩(wěn)定在垂直向上的位置，本文使用基于能量的公式來實現(xiàn)目標。 從勢能和動能的角度來考慮能量，當雙擺穩(wěn)定時勢能達到最大并且動能最小（向上位置）。

圖3 無障礙二階倒立擺系統(tǒng)仿真模型Fig.3 Simulation model of barrier-free double-inverted-pendulum system

首先將雙擺位置初始化為垂直向下（角度θ 為1 π），得到二階倒立擺無障礙模型預測控制結(jié)果，如圖4所示。

圖4 研究1：無障礙非穩(wěn)定初始的雙倒立擺的輸入輸出各變量變化Fig.4 Study 1：Changes in the input and output variables of the double-inverted-pendulum with the barrier-free and unstable initial

圖4 顯示了無障礙倒立擺系統(tǒng)的輸入輸出各狀態(tài)變量的變化，其中圖4（a）為擺桿動能在模型預測控制下的大小變化，從初始位置動能為零到穩(wěn)定時動能恢復到零；圖4（b）顯示了擺桿勢能的大小變化，由于初始位置為垂直向下，此時勢能為相反數(shù)，且數(shù)值達到最大， 隨著小車的運動及時間的推移，當雙擺位置穩(wěn)定到垂直向上時， 此時勢能達到最大，且為正值；圖4（c）為雙擺角度θ 的變化過程，以及圖4（d）在控制過程中施加力F 大小的變化。結(jié)果顯示，在模型預測控制下，系統(tǒng)在4 s 時達到穩(wěn)定狀態(tài)，并且各狀態(tài)變量也能夠限制在約束之內(nèi)。

為了增加控制的復雜度，在原有的倒立擺系統(tǒng)中增加了一個球形障礙物，半徑大小設(shè)置為0.2 m，位置位于小車正上方0.5 m 處， 包括設(shè)定點跟蹤任務，使得雙擺在所需位置必須出錯，系統(tǒng)模型如圖5所示，仿真結(jié)果如圖6 所示。

圖5 有障礙二階倒立擺系統(tǒng)仿真模型Fig.5 Simulation model of double-inverted-pendulum system with obstacles

圖6 研究2：有障礙非穩(wěn)定初始的雙倒立擺的輸入輸出各變量變化Fig.6 Study 2：Changes in the input and output variables of the double-inverted-pendulum with the obstacle and unstable initial stage

為了與上述實驗保持一致性，首先討論了當雙擺初始位置位于垂直向下時各狀態(tài)變量的變化。 由于增加了一個球形的障礙物，雙擺在小車運動過程中需要避免觸碰到障礙物，增加了整體的控制復雜度。 從仿真結(jié)果圖6 中可以看出，相較于無障礙物，系統(tǒng)達到穩(wěn)定時所需要的時間增加了接近1 s，說明控制小車來回運動多次才能使得雙擺處于穩(wěn)定狀態(tài)，圖6（a）動能和圖6（b）勢能變化也能體現(xiàn)出。 另外為了更好的突出小車的運動過程，增加了對小車位置的探討，如圖6（c）所示，小車從初始位置0 m運動到最終穩(wěn)定時的位置-0.8 m；圖6（d）給出了控制過程中輸入力F 的變化曲線。

另外，在接下來討論了當雙擺初始位置為垂直向上時，雙擺各狀態(tài)變量的預測變化，如圖7 所示，當初始位置為垂直向上（穩(wěn)定狀態(tài)）時，通過施加外力F 打破穩(wěn)定狀態(tài)后，可以看到系統(tǒng)相對于初始位置垂直向下的情況來說可以在更快的時間內(nèi)再次恢復到穩(wěn)定狀態(tài)。 即從穩(wěn)定狀態(tài)再次恢復到穩(wěn)定狀態(tài)所需時間比從不穩(wěn)定狀態(tài)到穩(wěn)定狀態(tài)所需時間少1.5 s 左右。

圖7 研究3：有障礙穩(wěn)定初始的雙倒立擺的輸入輸出各變量變化Fig.7 Study 3：Changes in the input and output variables of the initial double-inverted-pendulum with obstacles to stabilize

4 結(jié)語

本文利用Python 編程，首先基于拉格朗日方程對二階倒立擺系統(tǒng)進行了線性化建模，然后根據(jù)模型預測控制的控制原理進行了分析并設(shè)計了快速預測控制器。 在模型預測控制下，倒立擺系統(tǒng)的雙擺能夠穩(wěn)定保持在垂直向上的平衡位置，同時為了更好的評估控制性能，在原有的系統(tǒng)上進行了復雜化設(shè)計。 從仿真結(jié)果來看，模型預測控制可以準確的控制系統(tǒng)穩(wěn)定，并對各參量做出預測。