卡爾曼濾波器在倒立擺中的應用－IT工程師數位筆記本

文章出處

導讀：

就愛閱讀網友為您分享以下“卡爾曼濾波器在倒立擺中的應用”的資訊，希望對您有所幫助，感謝您對92to.com的支持!

?y，則平面倒立擺的X,Y?,y3??y,y4???xy1?y,y2?y?,x3??x,x4??選x1?x,x2?x

?1?x2,x?2?ux,x?3?x4,?x

方向動力學模型分別為：X方向:? Y方

??x4?fx(X)?bx(X)ux?1?y2,y?2?uy,y?3?y4,??y

向:?

???y4?fy(Y)?by(Y)uy

可見，平面倒立擺的線性化動力學模型是由結構相同的X，Y方向的動力學方程組成。通過線性化兩方向的耦合動力學模型的解耦得到兩相結構相同的X方向、Y方向的狀態空間表達式：

??AX?Bu?XxxxxX方向: ?x

?Yx?CxXx

???Xy?AyXy?Byuy

Y方向: ?

??Yy?CyXy

參數意義數值

m0小車質量 1.3282kgm1 下擺擺桿質量 0.2200kg m2 中擺擺桿質量 0.2200kg m3 上擺擺桿質量 0.1870kg l1 下擺質心到軸心的距離 0.304m l2 中擺質心到軸心的距離 0.304m l3 上擺質心到軸心的距離 0.226m L1 下擺與中擺間的距離 0.49m L2 中擺與上擺間的距離 0.49m

J1 下擺轉動慣量 0.004963 Kg*m*m J2 中擺轉動慣量 0.004963 Kg*m*m J3 上擺轉動慣量 0.004824 Kg*m*m f0 小車與導軌間的摩擦系數 22.9147 N*s/m

下擺與轉軸間的摩擦系數 0.007056 N*s*m f2 中擺與轉軸間的摩擦系數 0.002646 N*s*m f3 上擺與轉軸間的摩擦系數 0.002646 N*s*m

圖2.3平面倒立擺的參數

將以上平面一級倒立擺各物理參數代入以上的表達式，可得其參數矩陣分別為：

?0?0?Ax?Ay??0??0

00028.5

1000

0??1

?,Bx?By?[00??0?

01?2.95],Cx?Cy?[

]

從上式中可以看出，對于平面一級倒立擺系統，在平衡點附近經過線性化近似后，X方向和Y方向已經進行解耦。這樣，平面一級倒立擺是由一個八階的系統轉化為兩個獨立的相對簡單的四階系統，每個系統含有一個輸入，四個狀態變量，二個輸出變量，則分別為小車的位置和擺桿的角度，其大大降低了控制的難度。 2.3.2平面一級倒立擺系統定性分析

系統的特征方程為det??IA??0，經過計算得到系統的特征值：?1?0,?2,3??5.2826 系統有一個二重特征根在原點，一個特征根在復頻域的右半平面上，由定理可知系統是不穩定的。

首先系統的可控性計算，因為X、Y方向己經解禍，并且不考慮系統在X方向和Y方向上的模型差別，認為X方向和Y方向上具有相同的系數狀態方程，因此我們只計算X方向的可控性，Y方向上具有相同的可控性。

對一級倒立擺系統線性狀態方程式

M?[B

AB]

TT23TM??C(CA)(CA)(CA)???

以上兩個矩陣的秩 rank ( M ) = rank( V ) =4，與系統的階數相等，由定理可知，平面一級倒立擺系統是能控能觀的。

3 平面倒立擺的線性二次最優控制

3.1系統線性二次型最優控制原理

LQR (linear quadratic regulator)即線性二次型調節器，其研究對象是在現代控制理論中以狀態空間的形式來給出的線性控制系統，而目標函數則為對象狀態和控制輸入的二次型函數［16］。LQR最優設計是指設計是出的狀態反饋控制器K要使二次型目標函數J 取最小值，而 K由權矩陣Q、R與A、B的唯一決定，而A、B是由系統其本身的屬性來決定的，故此Q、R的選擇顯得尤為重要。LQR理論是現代控制理論中發展最早也是最為成熟的一種狀態空間設計法。特別指出的是，LQR可得到狀態線性反饋的最優控制規律易于形成閉環最優控制。

線性二次型調節器的問題在現代控制理論中占有非常重要的地位，受到了控制學界的普遍重視，它在倒立擺控制系統中的設計也得到廣泛的應用。使用這種方法的關鍵則在于對加權陣Q和R的選擇，來設計出一個線性二次型最優控制器。

對于線性系統的控制器的設計問題，如果其性能指標是狀態變量和控制變量的二次型函數的積分變量，這種動態系統的最優化問題則稱為線性系統的二次型性能指標的最優控制問題。我們知道線性二次型的最優解的形式是可以寫成統一的解析表達式去實現其求解的過程的規范化性，并可以簡單地來采用狀態線性反饋控制律，形成閉環最優控制系統，并且能夠兼顧多項性能指標，因此得到了特別的重視，已成為現代控制理論中發展較為成熟的一部分。

圖3.1 LQR最優控制原理

連續線性定常系統的狀態空間方程為：

?(t)?Ax(t)?Bu(t) (3.1) x

其中，x(t)為n維狀態向量，u(t)為r維控制向量，A，B分別是n×n，n×r維常

數矩陣。要去尋求控制量u?(t)使得如下二次型目標函數為最小。

?x?2

Qx?uRudt (3.2)

式中的Q、R分別為x和u的加權矩陣。根據極值原理，可以得到最優控制規律:

u??RBPx??Kx

(3.3)

式中，K為最優反饋增益矩陣；P為常值正定矩陣，其必須滿足黎卡提（Riccati）代數方程：

PA?AP?PB

RBP?Q?0 (3.4)

最優控制的性能指標決定于對稱矩陣P，而P矩陣又決定于A，B，Q和R的矩陣，A、B分別為系統的結構和參數所決定，故P值實際上取決于加權矩陣Q和R。我們設計狀態反饋控制器時，關鍵性問題就是二次型性能指標泛函數中加權矩陣Q和R的選取。

3.2 平面一級倒立擺的LQR控制器設計

3.2.1控制器設計

在第二章2.1節中我們已經得到了平面一級倒立擺的線性化數學模型，由在X,Y兩個方向的狀態空間方程完全相同互不相耦合，所以這里僅以X方向為例進行控制器的設計。在下面的描述中，為了簡便，我們略去各變量象征X方向的下標二，將X二記為X，依此類推。系統的狀態空間向量為X??X1X2X3X4?

??X??

?????

，

間方程和輸出方程來根據期望性能指標選定權重矩陣Q和R，利用其最優控制原理可以得到最優控制矩陣K(由十輸入量為標量，所以這里的K是一個四維行陣)。所以控制量可根據期望性能指標選定權重矩陣Q和R，利用最優控制原理就可以得到最優控制矩陣K(由十輸入量為標量，所以這里K是一個四維行陣)。所以控制量為：

u??Ku??(K1x?K2x?K3x?K4x)

為了能夠對小車施行定位控制，特設計位置參考輸入r，從位置量x到控制量的差異，故需要將參考輸入乘以反饋系統K，因此，一級平面倒立擺的線性二次最優控制系統框圖如圖3.2所示。

大連民族學院本科畢業設計（論文）

卡爾曼濾波器在倒立擺中的應用

學院（系）：機電信息工程學院

專業：自動化

學生姓名：趙明明

學號： 2007023424

指導教師：杜海英

評閱教師：

完成日期： 2011年6月

大連民族學院

摘要

倒立擺控制系統是一個非線性、多變量、高階次、強耦合、欠驅動的自然不穩定系統，是自動控制理論教學和研究中典型的物理模型。倒立擺是檢驗控制理論的經典設備。從控制角度來看，現在己有很多的方法實現了對倒立擺的控制，例如LQR最優控制、魯棒控制、模糊控制、神經網絡控制等。本論文主要針對卡爾曼濾波算法在基于LQR最優控制倒立擺系統上的應用，其主要內容如下:

首先，介紹了倒立擺系統的發展背景及研究意義。采用機理法建立平面一級倒立擺的數學模型，得出線性方程和動態方程，然后在倒立擺的平衡點附近進行系統的定性分析。其次，對線性二次型控制算法的優越性進行討論，并利用這一控制算法設計了平面一級倒立擺的控制器。隨后詳細介紹了卡爾曼濾波算法。再次，實現了平面一級倒立擺系統的仿真控制。特別指出的是，從實驗數據考察得知：加入卡爾曼濾波算法的最優控制的效果，其收斂速度快和穩定性要明顯好于未加入之前，并且也具有較小的超調量。最后，在上述的理論基礎上，我們參照倒立擺的實際系統參數，實現了平面一級倒立擺系統的實物實驗。

結論證明了基于卡爾曼濾波的最優控制系統大大降低了噪聲對系統的影響，同時有很好的提高了系統的穩定性、魯棒性和適應性。

關鍵詞：倒立擺卡爾曼濾波器線性最優二次控制器

Abstract

Handstand pendulum control system is a nonlinear, multivariable and high-order times, strong coupling, owe drive natural unstable system is, automatic control theory teaching and research in the typical physical model. Handstand pendulum is the classic equipment inspection control theory. From the control Angle, now has a lot of method to realize the control of the inverted pendulum, such as conventional LQR optimal control, robust control and fuzzy control, neural network control, etc. This thesis kalman filtering algorithm based on conventional LQR optimal control in the application of inverted pendulum system, its main contents are as follows:

Firstly, this paper introduces the development background of the inverted pendulum system and research significance. Adopt mechanism method to establish the plane level inverted pendulum, it is concluded that the mathematical model of linear equation and dynamic equation, and then in the balance inverted pendulum near the qualitative analysis system. Secondly, to linear quadratic control algorithm is discussed, and the superiority of the control algorithm design using a flat level inverted pendulum controller. Then introduces kalman filtering algorithm. Again, realized the plane level inverted pendulum system

simulation control. Specifically, from the experiment data investigation is that the join kalman filtering algorithm is the optimal control effect, its convergence speed and stability should not join was significantly better than before, and also it has a small overshoot. Finally, in the above theory basis, we refer to the actual system parameters inverted pendulum, realized the plane level inverted pendulum system of physical experiment.

Conclusion based on kalman filtering proved the optimal control system greatly reduces the noise on the system has very good effect, and improve the system stability and robustness and adaptability.

Key Words：inverted pendulum; kalman filter; Linear optimal second controller

摘要 .................................................................. I Abstract ................................................................. II

1 緒論 ................................................................... 1

1.1 倒立擺系統簡介 .................................................... 1

1.2濾波理論的發展及現狀 .............................................. 2

1.3課題任務及本文的主要工作 .......................................... 3

2 平面倒立擺系統建模 ...................................................... 5

2.1平面倒立擺簡介 .................................................... 5

2.2倒立擺的特性及工作原理 ............................................ 5

2.3固高平面一級倒立擺系統的數學模型建立 .............................. 7

2.3.1模型推導原理 ................................................ 7

2.3.2平面一級倒立擺系統定性分析 ................................. 10

3 平面倒立擺的線性二次最優控制 ........................................... 11

3.1系統線性二次型最優控制原理 ....................................... 11

3.2 平面一級倒立擺的LQR控制器設計 ................................... 12

4 卡爾曼濾波器在倒立擺中的應用 ........................................... 15

4.1卡爾曼濾波原理 ................................................... 15

4.2卡爾曼濾波器在倒立擺中的應用 ..................................... 17

5 系統仿真實驗與性能分析 ................................................. 20

5.1平面一級倒立擺軟件方案設計 ....................................... 20

5.2平面一級倒立擺硬件方案設計 ....................................... 20

5.3調試 ............................................................. 22

5.4 仿真曲線及其性能分析 ............................................. 23

6 結論 ................................................................... 29

參考文獻 ................................................................. 30

附錄 ..................................................................... 30

致謝 ..................................................................... 34

1 緒論

隨著現代科學技術的發展，控制工程所面臨的問題變得越來越復雜。實際的工程控制系統，由于種種原因存在一定的不確定性，而如何處理被控對象的不確定性一直是個有待研究的問題。倒立擺是控制理論的一個理想實驗平臺，針對其不確定性研究具有理論和實際的意義。倒立擺系統現在已被公認為是自動控制理論中典型的實驗設備，也是控制理論在教學和科研中極具典型的物理模型。倒立擺不僅僅是一種優秀的教學實驗儀器，同時也是進行控制理論研究的理想的實驗平臺。由倒立擺系統本身具有的高階次、不穩定、多變量、非線性和強耦合特性等特性，例如雙足行走的機器人與火箭垂直姿態和許多其他控制對象的最簡單最理想的模型，現在許多控制理論研究人員已把它視為典型的研究對象，并用它來描述線性和非線性控制領域中的變結構控制、無源性的控制、自由行走、非線性觀測器和非線性模型降階等控制理論的思想，并且不斷地挖掘出新的控制理論和控制方法，其相關成果已經在航空航天和機器人學等方面得到廣泛的應用。我們可以看到，倒立擺控制系統的研究既具有深遠的理論價值和實際意義，同樣具有重要工程的背景和實際的意義，可以提供從控制理論通往實踐現實的一座橋梁。

倒立擺開始于火箭發射器，最初的研究始是在二十世紀50年代，由美國的麻省理工學院控制理論的專家根據火箭發射助推器的原理設計出了一級倒立擺的實驗設備。倒立擺的控制技巧同雜技運動員倒立平衡表演有很多相似之處，這表明一個不穩定的被控對象，是可以通過人的直覺，來采取定性的手段，使之具有良好的穩定性。

從日常生活中我們所見到的任何重心在上支點在下的的物體控制問題，到空間飛行器和各類伺服云臺的穩定問題，都和倒立擺的控制具有很大相似之處，所以對其的穩定性控制在實際生活中也有很多得用場，例如中國海上鉆井平臺的穩定控制、衛星發射架的穩定控制、火箭的姿態控制、飛機的安全著陸、化工的生產過程控制等都屬于這類問題。針對上述的實際問題，啟發了我們要通過智能控制方法對倒立擺系統進行控制。所以我們對倒立擺系統的原理問題的研究具有重要的理論和實際意義，現在已成為控制理論中經典的研究課題。

1.1 倒立擺系統簡介

在控制理論發展的過程中，某一理論的正確性及其在實際應用中的可行性需要一個按其理論設計的控制器去控制一個典型對象來驗證這一理論，倒立擺就是這樣一個被控對象。倒立擺是一個多變量、非線性、強耦合、和不穩定的控制系統，我們對它引入一

個適當的控制方法使其變為一個穩定系統來檢驗其控制方法對不穩定、非線性系統的處理能力。在倒立擺系統的控制過程中，它即能有效地反映。例如可鎮定性、魯棒性、隨動性以及跟蹤性能等許多自動控制領域中的關鍵性問題。因此受到世界各國許多科學家的重視，從而用不同的控制方法控制不同類型的倒立擺，已成為最具有挑戰性的課題之一。

（1）我們知道機器人的站立和行走類似的雙倒立擺系統，當然第一臺的機器人是在美國問世，而且至今已有三十多年的歷史，機器人技術的關鍵就是機器人的行走控制問題，但至今仍未能很好解決。

（2）在火箭等飛行器的飛行過程中，我們為了保持其正確的姿態，就要不斷地對其進行實時控制。

（3）通信衛星要在我們預先計算好的軌道和預先確定的位置上運行時，我們要保持其具有穩定的姿態，使衛星天線一直保持指向地球，并使它的太陽能電池板直指于太陽。

（4）為了防止單級火箭在其拐彎時突然地斷裂，而研發誕生的柔性火箭或多級火箭，它的飛行姿態的控制同樣也可以用多級倒立擺控制系統來進行研究。

（5）偵察衛星中的攝像機的微微地抖動就會產生對攝像圖象質量很大的影響，我們為了提高其攝像的質量，就必須能自動地保持伺服天云臺的穩定問題，以消除其震動。

國際上每年都有很多關于倒立擺控制系統的研究論文進行發表，其中的很大一部分都是建立在計算機控制技術基礎上的進行仿真控制研究。并且都是主要以一級倒立擺作為其被控的對象進行仿真和研究的，而用二級倒立擺、平行倒立擺和三級倒立擺來仿真的文章則很少。我們知道三級倒立擺的控制系統一直作為控制理論界的經典難題被人們所關注，也一直是人們理論研究的熱點。目前，只有很少的一部分學者在對其實際物理擺進行設計和實驗控制理論進行研究。

1.2濾波理論的發展及現狀

倒立擺系統是一種典型的多變量、非線性和強藕合的系統，因此我們需要在進行控制理論驗證的同時要進行必要濾波。所謂濾波，就是從具有干擾的信號中得到有用信號的準確估計值。濾波理論就是在對系統的可觀測信號進行測量的基礎上，根據一定的濾波準則和采用某種統計的最優方法，對系統的狀態進行的估計理論和方法。早在1795年，為了測定出行星運動軌道，高斯(KGuass)提出了最小二乘估計法。在二十世紀40年代，weiner和Kolmogorov相繼獨立地提出維納濾波理論［12］。V.Kucer在1979年提

出了現代維納濾波方法［13］。卡爾曼(R.EKalman)在1960年提出卡爾曼濾波

(KalmanFiltering)理論，標志著現代濾波理論的形成。卡爾曼濾波理論的一經提出，立即受到了工程界的重視。隨著計算機技術廣泛發展，使得卡爾曼濾波理論應用在航空、航天等諸多領域。而工程中遇到的實際問題又使卡爾曼濾波的研究更加完善。多種二階廣義的卡爾曼濾波方法的提出及應用，進一步提高了卡爾曼濾波對非線性系統估計的性能，二階濾波方法考慮應用泰勒級數展開的二次項。引起的估計誤差，提高了對非線性系統的濾波的精度，但大大增加了其運算量，因此在實際中反而沒有EKF應用更加廣泛。與對非線性函數的近似相對比，對高斯分布的近似要更簡單得多。對于線性系統的UKf和EKf具有同樣的估計性能，但我們對于非線性系統的UKF方法是可以得到更好的估計和計算的。Juher［14］和Simon［15］將UKF的方法首先應用在了車輛的導航和定位，得到了一個較EKF方法和更好的結果。Merwe和Wan將其應用于神經網絡的訓練的實驗中，同樣也取得了良好的效果。PeihuTianwAnzhang也將其應用于視覺跟蹤。Stenger和Mendon將u舒方法同樣應用于基于模型的手跟蹤。以上研究均表明，當系統具有非線性特性時，UKF方法與傳統的EKF方法相比，對系統狀態的估計精度均有不同程度的提高〔21〕。

1.3課題任務及本文的主要工作

我們所要達到的技術任務指標為：（1）擺角的偏離峰值要<10；（2）系統的調節時間要<1min。在基本控制理論的基礎上，最終實現系統穩定，達到課題要求的誤差范圍之內。

本文的主要工作是在認識分析平面倒立擺系統的基礎上，對一級平面倒立擺系統進行了建模，并采用線性二次型最優控制算法實現了一級平面倒立擺的最優控制；采用卡爾曼濾波器對LQR控制器進行了改進，得到最佳的控制效果。具體工作內容如下：

1介紹了倒立擺系統的發展背景及研究意義。

2建立平面一級倒立擺的數學模型，得出線性方程和動態方程，然后在倒立擺的平衡點附近進行系統的定性分析。

3對線性二次型控制算法進行討論，并利用這一控制算法設計了平面一級倒立擺的控制器。

4介紹了卡爾曼濾波算法。設計加入卡爾曼濾波算法的最優控，實現了平面一級倒立擺系統的仿真控制。

5在上述的理論基礎上，我們參照倒立擺的實際系統參數，實現了平面一級倒立擺系統的實物實驗。 ?

2 平面倒立擺系統建模

2.1平面倒立擺簡介

當我們用手托起一根立起的竹竿時，我們會通過手臂的不斷移動來保持平衡，來使竹竿不倒。我們通過實驗儀器來將同樣的一根棍自由連接的狀態下立起來，和滑軌上的一個小車自由連接。我們通過控制小車在滑軌上左右移動的位移去保持這根棍一直保持立起不倒。

我們所用的實驗控制裝置是由固高科技(深圳)有限公司研發的平面倒立擺控制系統。倒立擺控制系統包含倒立擺本體、電控箱及由運動控制卡和普通Pc機組成的控制平臺等三大部分。

平面倒立擺本體由平面倒立擺本體由基座、同步帶、帶輪、交流伺服電機、滑竿、擺桿、滑套、滑臺、角編碼器、等一系列部分組成，如圖（2.1）所示。小車由電機通過同步帶驅動使其在滑桿上來回運動，保持其擺桿平衡。控制電機編碼器和角編碼器向運動控制卡來實現反饋小車和擺桿位置(線位移和角位移)。

圖2.1

電氣控制箱：電控箱內安裝有以下主要部件：交流伺服驅動器、I/O接口板、開關電源、開關和指示燈等電氣元件。

控制平臺：控制平臺主要有以下部分組成：與IBM PC，AT機兼容的PC機、GM400運動控制卡、GM400運動控制卡用戶接口軟件、演示實驗軟件。

2.2倒立擺的特性及工作原理

倒立擺從形式和結構上來看是多種多樣，但是幾乎所有的倒立擺都具有以下的特性：

（1）非線性

倒立擺是一個典型的非線性的復雜系統，實際中可以通過其線性化得到系統的近模型，在線性化處理后再控制。我們也可以利用非線性控制理論對其進行控制。倒立擺的非線性控制已成為一個研究的熱點。

（2）不確定性

主要的模型誤差以及機械傳動的間隙，各種阻力壓力等，實際控制中一般都會通過減少各種誤差來降低不確定性，如通過施加預緊力來減少皮帶或齒輪的傳動誤差，利用滾珠軸承來減少摩擦阻力等不確定因素。

（3）耦合性

倒立擺在各級擺桿之間，以及和運動模塊之間都有很強的耦合關系，在倒立擺的控制中一般都會在平衡點附近進行解耦計算，去忽略一些次要的耦合量。

（4）開環不穩定性

倒立擺控制系統的平衡狀態只有兩個，即在垂直向上的狀態與垂直向下的狀態，其中垂直向上為絕對不穩定平衡點，垂直向下則為穩定的平衡點。

圖2.1 平面倒立擺控制系統的硬件框圖

包括我們可知道計算機、伺服系統、運動的控制卡、倒立擺本體的光電碼盤反饋測量的元件等幾大部分，來組成了一個閉環的系統。圖中光電碼盤l是由伺服電機的自帶，我們可以根據該碼盤的反饋，并通過換算我們知道獲小車的位移和小車的速度信號。這些我們都是可以通過差分運算而得到。擺桿的角度可由光電碼盤測量出來，并且能直接反饋到控制卡，小車擺桿角度變化率信號是可以通過差分得到。計算機是從運動控制卡中來進行實時讀取數據，確定出控制決策(電機的輸出力矩)，并發送給其運動控制卡。運動控制卡經過的內部控制算法我們能實現它的控制決策來產生出相應的控制量，并由電機轉動來帶動小車運動，最終來保持擺桿平衡。

2.3固高平面一級倒立擺系統的數學模型建立

2.3.1模型推導原理

所謂控制系統的數學模型，就是利用數學結構公式來反映系統內部之間、內部與外部某些因素之間的精確的定量關系的表示，它是分析、設計、預報和控制一個系統的基礎。所以，要對一個系統進行控制研究，首先就要建立它的數學模型。當前建立數學的模型有兩種方法:一種是在了解和研究其對象的運動規律的基礎上，運用基本的物理定律，也就是用各個專門學科領域提出來的物質守恒和能量的守恒性原理和連續性的原理，系統結構的數據和數學手段的推導出一個系統內部輸入與輸出的狀態關系模型。這種方法得出的數學模型則稱為機理模型或解析模型，這種建立模型的方法稱為解析法或是機理建模。另一種是從系統的運行和實驗的數據建立系統的模型，這種方法則稱為系統辨識或實驗建模。實驗建模是通過在研究對象上加以一系列的研究者事先確定的輸入信號，激勵研究的對象并通過傳感器來進行檢測其可觀測的輸出問題，應用數學方法建立起系統的輸入和輸出之間的關系。這里面包括輸入信號的設計選取問題和輸出信號的精確檢測問題等內容。

固高GPIP2002型的平面運動倒立擺實驗系統由控制計算機、電機伺服驅動器和機械結構本體組成。其機械結構由X、Y方向的導軌、由伺服電機驅動可在X、Y平面內運動的基座小車、擺桿以及連接擺桿與小車之間的虎克鉸構成。將平面運動倒立擺抽象成小車和勻質擺桿組成的控制系統。如圖所示，依次建立全局坐標系OXYZ，以A原點、平行于OXYZ的局部坐標系Ax1y1z1與以A原點、z2軸沿擺桿方向的局部坐標系Ax2y2z2。裝置在虎克鉸處的兩個旋轉編碼器是用來測量擺桿繞X軸、Y軸的轉角。

圖2.2平面一級倒立擺的機械結構圖

在忽略空氣阻力以及摩擦力等后，可以將平面倒立擺系統看成平臺、均勻桿和質量塊組成，利用拉格朗日方程推導系統的動力學方程。由拉格朗日方程：

L(q,q)?T(q,q)?V(q,q)

式中L——為拉各朗日算子，q為系統的廣義坐標，T為系統的動能，V表示系統勢能。則方程是由廣義坐標qi和L表示為：

d?Ldt?qi

??L?q1

?fi

式中i?1,2,3?；fi為系統沿該廣義坐標方向上的外力，在平面一級倒立擺系統中，因為在廣義坐標?,?上均無廣義外力作用，所以以下等式成立：

d?Ldt??

??L??

?0，

d?Ldt??

??L??

為保證擺角與小車位移方向的一致性，令????y,???x。展開式2-3得到關于?x，

?y的方程組：

??x?f1?(?3??x??ys?yc?y?l??ys?xc?x?3gs?xc?y)/(c?y?3)l ?c?xc?y?6l??x

??y?f2?(?3??x??ys?xc?y?3l??xs?yc?y?3gs?yc?x)/(c2?x?3)l ?s?xs?y?3??c?y?2l??xy

取擺桿平衡位置時各變量的初值為零，記為

?xy,?y,y?y,???x,???y]T??[0,0,0,0,0,0,0,0,0,0]。 ?,??,?X?[x,?x,x

??x,???y在X，Y方向的進行解耦表達式：在平衡位置進行泰勒展開，并線性化，得?

??x??

3g4l

?x?

34l

??y???，?x

3g4l

?y?

34l

?? y

圖3.2平面一級倒立擺的線性二次最優控制框圖

?1000???0100T?Q?CC??R=1，

?0000????0000?

取

其中，而Q22?1 Q11?1 代表小車位置的目標函數的權重，

則是擺角的權重，輸入控制量 u 的權重 R 為 1。

仿真程序如下:

m0=0.8501;

ml=0.273420; m2=0.31078; Ll=0.17863; L2=0.27016; lenl=0.1503; len2=0.2136; g=9.8:

J1=0.008398875; J2=0.018249635; f0=0;

fl=0.008176; f2=0.003425; a0=m0+ml+m2; al=ml*lenl+m2*Ll; a2=mZ*lenZ;

bl=Jl+ml*len1^2+m2*LI^2; b2=J1+m2*len2^2;

M=[a0ala2:albla2*LI;a2a2*L1b2]: G=[1;0;0」;

03=zeros(3); i3=eye(3);

A=[0313:M/hM/f]: B=[0;0;0;m/G〕;

Q=diag[10，20，30，0，0，0]: R=1;

[K，S，E」=LQR(A，B，Q，R): x=(’K’); x=eval(x): C=norm(K); F=k/C;

程序的運行結果如下:

K=[-28.2843 -66.1019 -18.1729

-12.2045]

4 卡爾曼濾波器在倒立擺中的應用

4.1卡爾曼濾波原理

所謂的濾波就是從混合在一起的復雜的多種信號中提取出我們所需要的信號。對于具有確定的頻譜的確定性信號，可根據各個信號頻帶的不同，設置其具有相應頻譜特性的濾波器，如低通濾波器、帶阻濾波器，在使有用信號無衰減的通過而干擾信號受到抑制。此類濾波器則包括模擬濾波器和數字濾波器，物理和軟件算法等。對于沒有確定頻譜的隨機信號來說，則無法用常規的濾波方法提取或抑制信號。但我們應注意到信號具有確定的功率譜，所以可根據有用的信號和干擾信號的功率譜來設計濾波器，如維納濾波器等。但維納濾波方法的計算是比較復雜的，不易實現的。另一種方法就是從概率統計的角度去考慮，例如最小二乘估計的方法、最小方差估計卡爾曼濾波的方法等。而卡爾曼濾波的方法是從被提取的信號中有關量的測量中來通過算法估計，我們求出所需的信號。在估計的過程中，我們采用到了如下信息:量測方程、狀態方程、噪聲激勵的統計特性和量測誤差的統計特性。由于所用的信息都是時域內的量，所以卡爾曼濾波器是在時域內設計的并適用于多維系統。

我們簡單的來說，卡爾曼濾波器就是一個“ Optimal recursive data processing algorithm (最優化的自回歸數據處理算法)”。因此它是最優，效率最高甚至是最有用的濾波器。它己經廣泛應用了30多年，例如在機器人導航、控制、傳感器數據融合甚至在軍事方面的雷達系統以及導彈追蹤等領域內都已廣泛應用。近年來被應用在計算機的圖像處理，例如人臉識別，圖像的分割，圖像的邊緣檢測等。下面我們對濾波算法進行細致的描述。離散線性系統現有的濾波算法有連續的和離散的，接下來，我們要采用的濾波算法是一種基于離散型的卡爾曼濾波算法，在此，我們來對離散線性系統的濾波算法進行描述。設有隨機的動態系統，它的數學模型如下

x(k?1)?Y(k?1,k)x(k)?Tw(k) k?0 (4.1) z(k?1)?H(k?1)x(k?1)?v(k?1) k?0 (4.2)

式中: x?Rm是系統的狀態向量; z?Rm是系統的觀測向量; w?Rm是系統的噪

聲向量; v?Rm是觀測噪聲的向量; ??Rm

x?mr

是系統的狀態轉移矩陣; ??Rm?m是系

統的噪聲陣;H?Rm

?mx

是系統的觀測陣。關于系統的隨機性，我們假定系統的噪聲

初始狀態向量是?w?k?k?0?和觀測噪聲?v?k?1?k?0?是不相關的零均值高斯白噪聲，

m，維高斯隨機向量，且

E??w?k?wTE?vkv???TE?w(k)v?

?j????Q?k???k,j?

Q?k??0 (4.3) R?k?1??0 (4.4)

?j?1????R?k???k,j?

?j????0 (4.5)

E??x?0????x?0?,varx?0??P?0? (4.6)

TE?x0w???k?????0 （4.7） T

?E??x?0?v?k?1???0 （4.8）

上式中Q是系統噪聲的序列方差陣;R是觀測噪聲的序列方差陣。式 (4.1)和式 (4.2)描述的是線性離散時間隨機系統，也就是被估值系統。這組方程可以用下圖表示。

圖4.1被估值系統的方框圖

在式(4，1)和(4.2)中，若w(k)為系統確定性控制輸入，v(k+l)=O，則式(4.1) 和 (4.2)也就描述了一個確定性離散線性系統。對于這樣一個系統，如果系統是可觀測的，便可以通過構造一個狀態觀測器來觀測系統的狀態。而且我們知道狀態觀測器是一個以原系統為輸入的線性結構系統，其階數與原系統的階數相等。當然，如果系統的狀態向量的某些分量可有我們直接測得到，則觀測器階數可以低于原系統階數。基于上述的想法，我們對于隨機系統式(4.1)和(4.2)，我們也可以構造出一個以觀測序列為輸入的線性結構系統用來估計系統的狀態。此外，考慮到系統觀測具有隨機性的存在，使得不可能直接精確地觀測到系統狀態變量的個別分量，則狀態估計系統的階數不能降低，應為

mx。

圖4.2 估計系統的方框圖

4.2卡爾曼濾波器在倒立擺中的應用

滑模控制器在其實際運行過程中，需要獲得倒立擺的四個狀態變量。而在實際的系統中，經過一定距離的傳輸信號，或多或少都會受到外部的干擾，具體體現為系統的內部不確定因素導致系統的噪聲與觀測不精確導致在觀測信號中參雜的觀測噪聲等。雖然滑模控制器已具有良好的魯棒性，當噪聲干擾較小時我們可以對其進行抑制，但是當系統的噪聲干擾較大時，控制器就會受到極大影響，甚至失效。針對于這個問題，需要使用狀態估計的方法來從受污染的觀測信號中估計出系統的原狀態變量，并將估計值作為控制器的輸入。目前，狀態估計方法中應用最為廣泛的就是卡爾曼于二十世紀六十年代提出的最優狀態估計方法，即卡爾曼濾波器。卡爾曼濾波器利用反饋控制的思想對其過程進行估算，這樣數學結構比較簡單，而且非常適合于處理各種隨機過程。

在以上控制器設計的過程中,并未考慮系統噪聲和測量噪聲的影響,而實際上這些噪聲是存在的,并且對系統產生了影響。因此，我們要用卡爾曼濾波器對系統狀態值來進行預算估計,以得到消除噪聲影響的狀態值。設離散時間系統的差分方程如下描述:

xk?Axk?1?Buk?1?wk?1 ,

zk?Hxk?vk,式中的隨機信號wk 和vk是分別表示過程激勵的噪聲1和觀測的噪聲。

假設它們是相互獨立的, 則正態分布的白色噪聲為：

P?w??N?0,Q? ,P?v??N?0,R? 。

而知道卡爾曼濾波方程組為:

xk|k?1?Axk?1?Buk?1

Pk|k?1?APK?1A?Q

Kk?Pk|k?1HT??T?HPk|k?1HT?R?

???xk?xk|k?1?Kk?zk?Hxk|k?1? ????

Pk??1?KKH?PK|K?1

上式中PK|K?1為其先驗估計誤差的協方差，而Pk則為后驗估計誤差協方差, Kk為其

殘差增益, x0 和P0 則為初始狀態值。在對其倒立擺系統的運行狀態進行估計時, 我們

要考慮到實際的情況,所以我們取x0 = [0 0 0 0 ]T , P0 為4階的單位陣。這樣系統

不但可以很快收斂,而且能很大地降低白噪聲干擾對其系統性能的影響。

5 系統仿真實驗與性能分析

5.1平面一級倒立擺軟件方案設計

我們在對LQR最優控制分析之前，要通過狀態空間方程來分析系統的可控性和可觀性，來確定系統是否可以進行LQR最優控制，如果可以就能進行LQR最優控制對系統進行控制。最優控制的前提條件指系統是能控的。下面來判斷一下系統的能控能觀性：

在MATLAB中輸入：

Uc = ctrb（A ,B）;

Uo = obsv（A,C）;

rank（Uc）

rank（Uo）

得到的結果如下：

ans =

因為系統的能控矩陣和能觀測矩陣的秩都為4，所以該系統是能觀測的和能控的，因此可以給系統加上最優控制器使得閉環系統穩定，且滿所要求的足暫態性能指標。

在平面一級倒立擺數學模型已知的情況下，尋求一個最優的控制規律，使一級倒立擺從某一個初始狀態到達最優狀態并使系統的性能在某種意義上是最優的。在最優控制中，求取的目標函數值越小，反饋增益矩陣K的值也就越理想。可知道最優性能指標和K陣有關，而K矩陣又決定于A，B，Q和R的矩陣，A，B是分別由系統結構和參數所決定，故反饋增益K實際上就取決于加權矩陣Q和R。所以得出優化的Q和R陣也就可以得到反饋增益矩陣K。并對得到的Q和R陣的值來求出增益反饋矩陣K，由K構成閉環也就形成了一個新的狀態空間方程，從而使系統變得穩定。

5.2平面一級倒立擺硬件方案設計

固高實驗臺對一級倒立擺開發了LQR算法控制實驗，其運行過程如下:

（1）打開平面一級倒立擺控制程序如下圖所示：

圖 5.2 Simulink 環境下倒立擺系統的階躍響應仿真模型

圖5.3 Simulin環境下倒立擺系統加入卡爾曼濾波器的響應仿真模型

仿真模型的各模塊功能簡介

Reference input Constant （常值）保持參考輸入為固定值

Kx_x Gain （增益）將信號放大到一定倍數

Inverted pendulum State-space（狀態空間）表述倒立擺的數學模型 Optiaml gain Gain （增益）將系統狀態與控制參數相乘 To workspace To workspace（到工作空間）將系統狀態輸出到工作空間

Cart position Scope （示波器）實時觀察小車位置

Pole angle Scope （示波器）實時觀察擺桿角度

Cart velocity Scope （示波器）實時觀察小車速度

Pole angular velocity Scope （示波器）實時觀察擺桿的角速度

（2）在確認系數正確后，點擊“

（3）在編譯成功后，點擊“

（4）點擊“”進行編譯程序。 ”連接程序。 ”運行程序，在電機上伺服后，緩慢提起擺桿到平衡位置，在程序進入自動控制后松開手，得到實驗結果。

5.3調試

對平面一級倒立擺調試步驟如下：

（1）打開平面一級倒立擺控制程序如下圖所示：（進入MATLAB Simulink 實時控制工具箱“Googol Education Products”打開“Inverted Pendulum\Linear Inverted Pendulum\Linear 2-Stage IP Experiments” 中的“LQR Control Demo”）。

（2）“LQR Controller”為LQR 控制器，雙擊模塊設置參數：

圖5.4平面一級倒立擺LQR控制參數把以上仿真得到的控制參數輸入到LQR 控制器中。

5.4 仿真曲線及其性能分析

圖5.5 未加入卡爾曼濾波器系統控制器的輸出

1.5

0.5

-0.5

-1012345678910

圖5.6 加入卡爾曼濾波器系統控制器的輸出

圖5.7 未加入卡爾曼濾波器之前的小車位置曲線

圖5.8 加入卡爾曼濾波器之后的小車位置曲線

12345678910

圖5.9 未加入卡爾曼濾波器之后的小車速度輸出曲線

0.15

0.1

0.05

-0.05

-0.1

012345678910

圖5.10加入卡爾曼濾波器之后的小車速度輸出曲線

0.50.4

0.30.20.10-0.1-0.2-0.3

012345678910

圖5.11 加入卡爾曼濾波器之后的小車擺桿角度輸出曲線

0.5

0.4

0.3

0.2

0.1

-0.1

-0.2

012345678910

圖5.12加入卡爾曼濾波器之后的小車擺桿角度輸出曲線

12345678910

圖5.13未加入卡爾曼濾波器之后的小車擺桿角加速度曲線

1.5

0.5

-0.5

-1

012345678910

圖5.14加入卡爾曼濾波器之后的小車擺桿角加速度曲線

圖5.5 是未加入卡爾曼濾波器系統控制器的輸出曲線，表明系統的輸出存在一定誤差，且容易受到噪聲的干擾，影響系統的穩定。圖5.6是加入卡爾曼濾波器系統控制器的輸出曲

線，表明了系統加入卡爾曼濾波器后系統的輸出變得平滑，系統達到平衡，性能明顯變

好。圖5.8，圖5.10，圖5.12，圖5.13是分別反映了系統加入卡爾曼濾波器后，小車的位

置，速度，擺桿角度和角加速度的輸出曲線，分別表明了濾波后的狀態估計值大大降低了系

統受噪聲的影響,提高了系統會的穩定性、魯棒性和動態性能。

6 結論

本項目的主要工作就是針對設計控制器使倒立擺系統穩定并可以定位在特定位置的這一研究方向，對倒立擺系統智能控制算法的展開研究，運用MATLAB/simulink 對控制系統進行仿真設計以及實物系統的控制。從而證明倒立擺系統智能控制方法的可行性，來設計控制器的穩定性、魯棒性和適應性。圍繞平面一級倒立擺系統，嘗試用多種方法來建立倒立擺系統的數學模型，并推導出它們的微分方程以及線性化后的狀態方程，分析倒立擺控制系統的能控性、能觀性及相對能控性，對倒立擺控制系統進行控制方法的研究及系統的定性分析。

分析并討論倒立擺系統的多種控制方法，并用目前應用較為普遍的LQR控制方法實現倒立擺系統控制，尋找最優控制器參數，實現倒立擺系統最優控制。

把LQR控制方法和卡爾曼濾波聯系起來應用，提高了LQR最優控制在實際控制中的應用。降低了噪聲對系統的影響,而且顯著地提高了其系統的魯棒性，LQR最優控制是利用廉價成本可以使原系統達到較好的性能指標，而且方法簡單也便于實現。

在一級倒立擺穩擺控制系統中,擺桿的角度信號容易受到測量噪音污染,當通過差分獲取角速度時,該噪音被放大,導致穩擺控制器的輸出會產生較大的隨機波動,同時由于電機作為執行器,也混入了較大的過程噪音.這兩種噪音都會使系統出現較強抖動,既不利于擺桿的穩定,也縮短了電機的壽命.對此,將穩態卡爾曼濾波器與漸消記憶法結合了起來,找到了確定穩態卡爾曼濾波器增益矩陣的有效方法,既克服了模型誤差,又滿足了實時性要求,即減小了電機抖動,又取得了較好的效果。

參考文獻

① 俞金壽. 過程控制系統和應用. 北京: 機械工業出版社, 2003, 124-125 ② 解學書. 最優控制理論與應用. 北京: 清華大學出版社, 1996, 20-35 ③ 張靜等. 在自控系統中的應用. 北京: 電子工業出版社, 2007, 156-201

④ 姜捷, 江艷君. 控制系統分析設計和應用, MATLAB語言的應用. 北京: 化學工業出版社,

2003, 102-152

⑤ 盧伯英, 于海勛, 譯. 現代控制工程(第四版). 北京: 電子工業出版社, 2003 ⑥ 段廣仁. 線性系統理論(第2版). 沈陽: 哈爾濱工業大學出版社, 2004 ⑦ 深圳固高公司. GIP21002L倒立擺實驗指導書, 2001

⑧ 劉海龍. 倒立擺實驗系統的設計與研究: [碩士學位論文]. 2006 ⑨ 陳桂明. 應用MATLAB建模與仿真. 北京: 科學出版社, 2001

⑩ 史曉霞, 張振東. 二級倒立擺系統數學模型的建立及意義. 河北工業大學學報, 2001,

3(6): 5-7

? 從爽, 張東軍. 單級倒立擺三種控制方法的對比研究. 系統工程與電子技術, 2001, 23(1):

47 – 49

? 褚建. 關于最優調節器LQR 魯棒性的討論. 控制與決策, 1992, 7(6): 478-481

? 潘勝強. 擾動下線性二次型反饋調節器的魯棒性與靈敏性. 控制理論與應用, 1999, 7(1):

98-104

? 史曉霞. 二級倒立擺系統的建立及意義. 河北工業大學報, 2001, 30(5): 48-51 ? 李巖. 二級倒立擺控制系統分析. 沈陽工業學院學報, 1999, 18(5): 86-91

? 張飛舟,沈程智,范躍祖.擬人智能控制三級倒立擺,計算機工程與應用［J］,2000,2:17-20. ? 李洪興,苗志宏,王加銀.四級倒立擺的變論域自適應模糊控制.中國科學

[J]2002.2,32(1):65-75

? N.D. Anha, H. Matsuhisab,L.D.Vieta,M.Yasuda.Vibration control of an inverted

pendulum type structure by passive mass–spring-pendulum dynamic vibration absorber. Journal of Sound and Vibration[J] 307 (2007) 187–201 ? (24)Hirotaka Yoshida, Kenichi Kawata,Yoshikazu Fukuyama.A Particle Swarm

Optimization for Reactive Power and Voltage Control Considering Voltage Security Assessment.IEEE TRANSACTIONS ON POWER SYSTEMS,VOL.15, NO. 4, NOVEMBER 2000 ? (25)Liang Zhao,Yupu Yang. PSO-based single multiplicative neuron model for time

prediction. Department of Automation, Shanghai Jiao Tong University, Shanghai 200240,PR China

附錄

TM?1/2Mcar*x?[t];xpend1?x[t]?l1*Sin[?1[t]];ypend1?l1*Cos[?1[t]];

tpend1?1/2*m1*((?txpend1)^2)?1/2*(1/3*m1*l1^2)*?1[t]^2;xspend2?x[t]?2*l1*Sin[?1[t]]?l2*Sin[?2[t]];yspend?2*l1*Cos[?1[t]]?l2*cos[?2[t]];

tspend2?1/2*m2*((?txpend2)^2?(?tyspend2)^2)?1/2*(1/3*m2*l2^2)*(?2[t]^2);xmass?x[t]?2*l1*Sin[?1[t]];ymass?2*l1*Cos[?1[t]];

tmass?2*l1*m3*((?txmass)^2?(?tymass)^2);v?m1*g*yspend?m2*g*yspend?m3*g*ymass;Simplify[v];

lang?TM?tspend1?tspend2?tmass?v;simplify[lang];

ldad???1?[t]lang;simplify[ldad];

f1??tldad???1[t]lang;Simplify[f1];

ldbd???2?[t]lang;Simplify[ldbd];

f2??2ldbd???2[t]lang;Simplify[f2];

Solve[?f1??0,f2??0?,?1[t],?2[t]];?

add??1[t]/.;?

bdd??2[t]/.;

k11??x?t?add/.x[t]?0/.?1[t]?0/.?2[t]?0.x?t]?0/.?1[t]?0/.

?2[t]?0/.x??[t]?0;

k12???1[t]add/.x[t]?0/.?1[t]?0/.?2[t]?0.x?t]?0/.?1[t]?0/.

?2[t]?0/.x??[t]?0;

k13???2[t]add/.x[t]?0/.?1[t]?0/.?2[t]?0.x?t]?0/.?1[t]?0/.

?2[t]?0/.x??[t]?0;

k14??x?[t]add/.x[t]?0/.?1[t]?0/.?2[t]?0.x?t]?0/.?1[t]?0/.

?2[t]?0/.x??[t]?0;

k15???1?[t]add/.x[t]?0/.?1[t]?0/.?2[t]?0.x?t]?0/.?1[t]?0/.

?2[t]?0/.x??[t]?0;

k16???2?[t]add/.x[t]?0/.?1[t]?0/.?2[t]?0.x?t]?0/.?1[t]?0/.

?2[t]?0/.x??[t]?0;

k17??x???[t]add/.x[t]?0/.?1[t]?0/.?2[t]?0.x?t]?0/.?1[t]?0/.

?2[t]?0/.x??[t]?0;

k21??x[t]bdd/.x[t]?0/.?1[t]?0/.?2[t]?0.x?t]?0/.?1[t]?0/.

?2[t]?0/.x??[t]?0;

k22???1[t]bdd/.x[t]?0/.?1[t]?0/.?2[t]?0.x?t]?0/.?1[t]?0/.

?2[t]?0/.x??[t]?0;

k23???2[t]bdd/.x[t]?0/.?1[t]?0/.?2[t]?0.x?t]?0/.?1[t]?0/.

?2[t]?0/.x??[t]?0;

k24??x?[t]bdd/.x[t]?0/.?1[t]?0/.?2[t]?0.x?t]?0/.?1[t]?0/.

?2[t]?0/.x??[t]?0;

k25???1?[t]bdd/.x[t]?0/.?1[t]?0/.?2[t]?0.x?t]?0/.?1[t]?0/.

?2[t]?0/.x??[t]?0;

k26???2?[t]add/.x[t]?0/.?1[t]?0/.?2[t]?0.x?t]?0/.?1[t]?0/.

?2[t]?0/.x??[t]?0;

?k27??x??[t]bdd/.x[t]?0/.?1[t]?0/.?2[t]?0.x?t]?0/.?1[t]?0/.

?2[t]?0/.x??[t]?0;?

致謝

日月光華，旦復旦兮，四年荏苒的大學時光就這樣匆匆而流過，在不經意朝夕之間，很快就指向了畢業。現在到,畢業論文已經做完，這是我在這四年的最后學習過程之中，自己在控制學科領域所作的微不足道的工作和思索的總結。我深深感悟到能夠探求與領略到一點點科學的華采與真諦!

本論文是在杜海英老師的悉心指導下完成的。杜老師的學識淵博，風氣嚴謹，平易可親，誨人不倦，她對問題清晰敏銳的學術洞察力使我受益匪淺。同時也促成我在以后的學習和工作中的分析及解決問題能力的進一步提高。杜老師除了在我的學業上對我全面指導之外，更言傳身教，教我如何為人處事。值此在論文完成之際，謹向杜海英老師致以衷心的感謝!

感謝我最親愛的家人：可親可敬、含辛茹苦的父親、母親以及關心體貼我的姐姐。他們一直在默默地給予著我不遺余力的關心、支持與幫助。沒有他們長期的關愛與教導，我的學業絕不可能完成的。在論文付梓之際，特向他們致以我的最誠摯的敬意與謝意!

感謝我摯親摯愛的同學們和朋友們，是你們陪伴我走過這人生旅途中近一千個日日夜夜，是你們陪伴我一同譜寫這青春歲月中最華麗的篇章，我將永遠地記憶那些留在我生長卷之中的點點滴滴，濃墨重彩!

感謝我的母校能為我所提供的良好與充足的教育資源，還有各位老師的諄諄教導。最后，謹向審閱本文的各位專家、教授和老師們的辛勤工作致以衷心感謝!

百度搜索“就愛閱讀”,專業資料,生活學習,盡在看文倉kanwencang.com,您的在線圖書館
歡迎轉載：http://www.kanwencang.com/xuexi/20170209/99413.html

文章列表