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

（山西大同大學(xué) 機(jī)電工程學(xué)院，大同 037003）

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

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

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

1 理論

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

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

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

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

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

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

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

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

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

小車(chē)和兩根擺桿的勢(shì)能分別為

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

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

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

其中：

1.2 模型線性化

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

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

2 模型預(yù)測(cè)控制

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

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

模型預(yù)測(cè)控制解決問(wèn)題的第一步就是模型的離散化， 隨后將在每個(gè)采樣時(shí)刻求解離散化問(wèn)題，通常使用以下形式的離散模型：

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

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

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

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

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

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

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

3 仿真研究

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

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

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

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

為了增加控制的復(fù)雜度，在原有的倒立擺系統(tǒng)中增加了一個(gè)球形障礙物，半徑大小設(shè)置為0.2 m，位置位于小車(chē)正上方0.5 m 處， 包括設(shè)定點(diǎn)跟蹤任務(wù)，使得雙擺在所需位置必須出錯(cuò)，系統(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

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

另外，在接下來(lái)討論了當(dāng)雙擺初始位置為垂直向上時(shí)，雙擺各狀態(tài)變量的預(yù)測(cè)變化，如圖7 所示，當(dāng)初始位置為垂直向上（穩(wěn)定狀態(tài)）時(shí)，通過(guò)施加外力F 打破穩(wěn)定狀態(tài)后，可以看到系統(tǒng)相對(duì)于初始位置垂直向下的情況來(lái)說(shuō)可以在更快的時(shí)間內(nèi)再次恢復(fù)到穩(wěn)定狀態(tài)。 即從穩(wěn)定狀態(tài)再次恢復(fù)到穩(wěn)定狀態(tài)所需時(shí)間比從不穩(wěn)定狀態(tài)到穩(wěn)定狀態(tài)所需時(shí)間少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é)語(yǔ)

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