自平衡控制系統(tǒng)穩(wěn)定性分析與驗證

張 毅，張 磊，羅 元

(重慶郵電大學智能系統(tǒng)及機器人研究所，重慶400065)

0 引言

自平衡控制系統(tǒng)的主要任務是精確地獲取系統(tǒng)當前準確的傾角及實時產(chǎn)生合適的控制量，使系統(tǒng)維持在平衡狀態(tài)。由于自平衡控制系統(tǒng)自身的優(yōu)點，廣泛應用在火箭發(fā)射中的垂直度控制、倒立擺控制，兩足行走機器人以及兩輪電動雙輪車直立控制等方面，因此，對其穩(wěn)定的研究具有重要意義［1］。

對于自平衡控制系統(tǒng)，主要任務是準確獲取當前系統(tǒng)的傾角，采取一定的策略快速地調(diào)節(jié)系統(tǒng)保持穩(wěn)定。通常對自平衡系統(tǒng)的理論及穩(wěn)定性分析比較復雜，在系統(tǒng)傾角獲取上一般采用卡爾曼濾波、互補濾波等算法獲取系統(tǒng)當前的傾角數(shù)據(jù)［2］，但誤差仍然存在。對于后續(xù)系統(tǒng)平衡的控制，一般采用模糊比例積分微分(proportion integral differential，PID)控制、線性二次型調(diào)節(jié)器或自適應控制器等方法控制系統(tǒng)達到并維持平衡狀態(tài) 。為提高控制精度，部分算法需細分控制范圍，但會導致控制規(guī)則呈幾何級數(shù)增加，即所謂的“規(guī)則爆炸”問題［4］，影響控制實時性。

本文針對自平衡系統(tǒng)采用重心運動方程來分析系統(tǒng)的穩(wěn)定性及維持系統(tǒng)穩(wěn)定的控制方法，通過實際和理論分析，降低維持系統(tǒng)穩(wěn)定性分析和算法的復雜性，同時根據(jù)所用的獲取傾角數(shù)據(jù)的方案，配合理論分析與校正得到當前更精確的傾角數(shù)據(jù)。同時為完成對系統(tǒng)平衡的調(diào)節(jié)，用濾波后的傾角和陀螺儀數(shù)據(jù)分別配以對應的參量，組成比例微分(proportion differential，PD)控制，計算出驅(qū)動機構(gòu)的控制量，完成系統(tǒng)的調(diào)節(jié)過程。

1 數(shù)學模型的建立及簡化

根據(jù)自平衡控制系統(tǒng)的定義與應用場合，其行為特性與倒立擺有很大的相似性［5］，故在分析自平衡控制系統(tǒng)的特性時可以參考對倒立擺穩(wěn)定性分析的控制方法。但相比于倒立擺，自平衡控制系統(tǒng)有其自身的特點，由于其調(diào)整平衡機構(gòu)的質(zhì)量相對于整個系統(tǒng)的質(zhì)量很小，在分析過程中可以忽略驅(qū)動機構(gòu)的質(zhì)量，這樣不但簡化了理論分析，而且簡化算法的設計，降低控制系統(tǒng)運算的復雜程度。下面將對比倒立擺的建模，引出自平衡控制系統(tǒng)的建模，如圖1所示。

圖1 倒立擺的受力分析Fig.1 Stress analysis for Inverted Pendulum

圖1中，對其水平和垂直方向受力分析、結(jié)合剛體動力學方程可得出倒立擺的運動方程［6］:

(1)-(2)式中:x為小車的相對基準位移;x˙為小車的速度;x為小車的加速度;θ為擺線對垂線的角位移;˙θ為擺線對垂線的角速度;¨θ為擺線對垂線的角加速度;J為擺繞其重心的轉(zhuǎn)動慣量;m為擺的質(zhì)量;M為小車的質(zhì)量;l為擺重心到轉(zhuǎn)軸之長;H為小車對擺的水平反力;V為小車對擺的垂直反力;c為擺轉(zhuǎn)動時的摩擦系數(shù);b為小車的滑動系數(shù);F為作用在小車上的外力。

由傳統(tǒng)的對自平衡系統(tǒng)的分析方法可知，其分析方法復雜，運算求解時間長，為此特簡化自平衡系統(tǒng)，降低分析的復雜度。忽略驅(qū)動機構(gòu)的質(zhì)量M，簡化后的倒立擺模型，即等效的自平衡控制系統(tǒng)模型如圖2所示，對模型受力分析如圖3所示。

圖2 自平衡控制系統(tǒng)等效圖Fig.2 Equivalent chart for self-balancing control system

圖3 自平衡控制系統(tǒng)受力分析圖Fig.3 Stress analysis for self-balancing control system

將模型等效為可繞底部支點左右轉(zhuǎn)動的倒立擺，其重心距底部距離為L，質(zhì)量為m。假設由于外力的干擾引起系統(tǒng)向右傾斜產(chǎn)生角加速度x(t)，偏離數(shù)值方向一個角度θ，忽略驅(qū)動機構(gòu)的質(zhì)量，控制系統(tǒng)檢測到此傾角后立即控制驅(qū)動機構(gòu)驅(qū)動底部支點向右加速運動，此時，在質(zhì)心m所在位置進行分析，它就會受到額外的慣性力，該力作用在系統(tǒng)質(zhì)心上，與底部驅(qū)動調(diào)節(jié)的加速度方向相反，大小為macosθ，且與a成正比，故在質(zhì)心m運動的速度方向上。

等式兩邊消去m，得

在實際調(diào)節(jié)過程中，調(diào)節(jié)速度很快，通常偏離平衡位置很小角度，故運動方程簡化為

2 穩(wěn)定性分析與極點配置

2.1 實際模型穩(wěn)定性分析

自平衡控制系統(tǒng)是一個高階次、非線性、不穩(wěn)定的系統(tǒng)，在實際的單擺模型中，當物體偏離平衡位置時，受到回復力(重力的切向分力)與物體的位移方向呈鈍角α，故單擺在偏離平衡位置時，重力的切向分力對擺動物體做負功，同時由于空氣阻力及其它因素，最終使擺動物體停止在平衡位置。而對于自平衡控制系統(tǒng)，其重心在支撐點之上，重心位于支撐點正上方時，系統(tǒng)處于不穩(wěn)定的平衡狀態(tài)，由于制造誤差和其他外界干擾因素的存在，物體很難停止在支撐點的正上方，一旦偏離支撐點的正上方，擺動物體重力的切向分量與其位移s呈銳角β，即對脫離正上方支撐點的物體做正功，加速擺動物體的偏離程度。2種狀態(tài)分別如圖4和圖5所示。

圖4 單擺穩(wěn)定性分析示意圖Fig.4 Schematic diagram for stability of pendulum

圖5 倒立擺穩(wěn)定性分析示意圖Fig.5 Schematic diagram for stability of Inverted pendulum

2.2 系統(tǒng)極點配置

由公式(5)，當自平衡系統(tǒng)靜止時，a(t)=0，化簡得

系統(tǒng)傳遞函數(shù)為

驅(qū)動機構(gòu)通過調(diào)節(jié)底部支點加減速運動來完成調(diào)節(jié)平衡的過程。驅(qū)動機構(gòu)進行調(diào)節(jié)時，系統(tǒng)會在擺動的質(zhì)心處就會受到額外的慣性力，該力與底部調(diào)節(jié)機構(gòu)運動的加速度方向相反，大小成正比(見圖3)，此時擺動質(zhì)心處受到的回復力為

在實際的調(diào)節(jié)過程中，調(diào)節(jié)速度很快，通常偏離平衡位置角度很小，故對運動方程線性化為

由(8)式，如果k1＞g，回復力的方向便與位移方向相反，當物體偏離平衡位置時，受到指向平衡位置的回復力，使系統(tǒng)重心趨于平衡位置。

為使系統(tǒng)盡快在平衡位置穩(wěn)定下來，還需增加阻尼力。增加的阻尼力與偏角的速度成正比，方向相反。因此，(8)式變?yōu)?/p>

增加阻尼項后，把倒立擺模型等效成單擺模型，能夠穩(wěn)定在平衡位置附近。因此，可得到控制系統(tǒng)的加速度控制算法。

(8)-(11)式中:θ為偏離平衡位置的傾角;θ'為角速度;k1，k2為比例系數(shù)。兩項相加后作為系統(tǒng)調(diào)節(jié)平衡的控制量。只要保證k1＞g，k2＞0的條件下，可以使得自平衡系統(tǒng)像單擺一樣維持在近似直立的狀態(tài)。k2決定了回到平衡位置的阻尼系數(shù)，選取合適的阻尼系數(shù)可以保證系統(tǒng)盡快穩(wěn)定在平衡位置;k1決定了系統(tǒng)是否能夠穩(wěn)定到垂直平衡位置，它必須大于重力加速度。

得出自平衡控制系統(tǒng)保持穩(wěn)定性的條件和控制系統(tǒng)的加速度控制算法后，對控制系統(tǒng)引入比例、微分反饋控制，如圖6所示。

在圖6中，引入了角度反饋控制，與角度成比例的控制量為比例控制;與角速度成比例的控制量為微分控制。其中，微分參數(shù)相當于阻尼力，可以有效抑制系統(tǒng)的震蕩。引入比例、微分控制后系統(tǒng)穩(wěn)定性分析如下。

系統(tǒng)的傳遞函數(shù)為

此時，系統(tǒng)的極點為

系統(tǒng)要保持穩(wěn)定，2個極點都位于S平面的左半平面［7］。要滿足條件，需要k1＞g，k2＞0。因此，當k1＞g，k2＞0時，自平衡控制系統(tǒng)便可以保持平衡的狀態(tài)。在實際測試中通過適當調(diào)整比例和微分的系數(shù)，可使系統(tǒng)快速回到平衡位置，并維持穩(wěn)定狀態(tài)。

圖6 引入比例、微分反饋后的系統(tǒng)框圖Fig.6 System block diagram for the introduction of proportional，differential

3 控制策略及結(jié)果分析

3.1 獲取與處理傾角信息

對自平衡系統(tǒng)穩(wěn)定性的分析，有助于采用合理控制策略對系統(tǒng)穩(wěn)定性的控制。由于實際自平衡系統(tǒng)的振動及硬件電路的干擾，會對加速度傳感器信號的輸出產(chǎn)生很大的干擾。陀螺儀動態(tài)性能很好，不易受振動和速度的影響，但容易產(chǎn)生漂移誤差，很小誤差經(jīng)過積分運算后會產(chǎn)生很大誤差，因此，都不適合單獨使用。結(jié)合各自的優(yōu)缺點及系統(tǒng)調(diào)解平衡所需的條件，同時采用2種傳感器，經(jīng)過算法處理共同完成角度檢測和產(chǎn)生控制量的任務。

為精確獲取傳感器輸出的角度信息，采用卡爾曼濾波對傳感器輸出的數(shù)據(jù)進行處理。在應用卡爾曼濾波算法過程中，先對陀螺儀輸出的角速度信息進行積分，求得陀螺儀傳感器對應的角度信息Angle_gyro:

(14)式中，Gyro_data為陀螺儀輸出的角速度信息，對比加速度傳感器輸出的角度信息，求得當前的角度誤差為

(15)式中:Angle－error為當前的角度誤差;Angle_data是加速度傳感器輸出的經(jīng)過計算的角度信息，依據(jù)卡爾曼濾波的第3個公式，并進行簡化得

(16)式中，等號左邊的Angle就是經(jīng)過運算之后當前比較準確的角度信息，等式右邊的Angle是上一次經(jīng)過卡爾曼濾波后比較準確的角度信息。在整個卡爾曼濾波運算過程中，需要不斷遞推卡爾曼增益Kg以及當前的協(xié)方差P，以便進行下一次角度的計算和更新。同時也要更新b，求得當前的、經(jīng)過濾波后的真實角速度Gyro_real，具體控制過程如圖7所示。

圖7 傳感器信息處理方法Fig.7 Processing method of Sensor information

利用加速度傳感器所獲得的角度數(shù)據(jù)Angle_data，與陀螺儀積分之后的角度Angle_gyro進行比較，將比較的誤差經(jīng)過比例1/Tg放大之后與陀螺儀輸出的角速度數(shù)據(jù)疊加，抵消陀螺儀的積分誤差，之后再進行積分，此時獲取的角度更接近真實的系統(tǒng)傾角。

3.2 產(chǎn)生控制量

由系統(tǒng)穩(wěn)定性分析與極點配置，系統(tǒng)快速回到平衡位置并維持穩(wěn)定需引入比例和微分控制。通過系統(tǒng)對傾角信息的獲取及處理，已得到精確的角度和角速度信息。在數(shù)學運算關系上，對角度微分得到角速度。系統(tǒng)通過獲取傳感器的信息并處理，分別得到精確的角度和角速度信息，角度信息看成是比例項的一部分，則獲取的角速度信息為微分項的一部分，二者配合適當?shù)南禂?shù)組成比例微分PD控制器，便可以產(chǎn)生恰當?shù)目刂屏?，控制?qū)動機構(gòu)完成調(diào)解過程。具體控制過程如圖8所示。

圖8中，k1和k2分別是比例系數(shù)和微分系數(shù)，具體意義和功能上面已論述，產(chǎn)生控制量的表達式為

(17)式中，Kp和Kd相當于圖8中的k1和k2。適當?shù)恼{(diào)節(jié)Kp和Kd，便可得到合適的控制量，控制驅(qū)動機構(gòu)完成平衡的調(diào)整。

圖8 產(chǎn)生控制量的方法Fig.8 Method for generating control quantity

3.3 測試結(jié)果及分析

在測試平臺上搭建自平衡控制系統(tǒng)，精確獲取系統(tǒng)角度和角速度信息，適當調(diào)節(jié)控制量相關的參數(shù)，便可完成對自平衡系統(tǒng)穩(wěn)定性的控制。將自平衡控制系統(tǒng)運行中的參數(shù)實時傳送到上位機，便可觀察各參數(shù)之間的變化，濾波前的角度與濾波后的角度各放大1 000倍后對比如圖9所示。

圖9 濾波前后的角度對比Fig.9 Angle Contrast before and after filtering

圖9中，下方曲線是濾波前的角度數(shù)據(jù)，上方曲線是經(jīng)過濾波和誤差比較放大后的角度數(shù)據(jù)。從圖9中可知，由于機械振動對加速度輸出的影響及硬件電路電磁干擾的存在，濾波前采集到的角度數(shù)據(jù)混有高頻干擾信號，經(jīng)過濾波后，角度數(shù)據(jù)比較光滑，更為接近實際的角度數(shù)據(jù)。

系統(tǒng)調(diào)節(jié)產(chǎn)生的控制量需要當前比較準確的角度數(shù)據(jù)和角速度數(shù)據(jù)，故需對采集的角度和角速度數(shù)據(jù)進行濾波，獲得相對精確的數(shù)據(jù)。各放大1 000倍后上傳到上位機，圖10為濾波前后角度波形與濾波后陀螺儀數(shù)據(jù)變化情況，中間2條頻率比較低的波形為濾波前后的角度數(shù)據(jù)，位置關系見圖9。頻率和幅值比較高的曲線是濾波后陀螺儀波形。

濾波后的角度、角速度與控制量之間的波形如圖11所示。圖11中，角度數(shù)據(jù)放大100倍，陀螺儀數(shù)據(jù)放大10倍，控制量不放大。其中，處于上方頻率較低的波形為濾波后的角度數(shù)據(jù)，中間頻率比較低的波形為產(chǎn)生的控制量，下方的頻率和幅值比較高的波形為濾波后的陀螺儀數(shù)據(jù)。由圖11可知，由PD控制器產(chǎn)生控制量波形與角度數(shù)據(jù)波形比較相近，主要原因是角度數(shù)據(jù)在控制量的產(chǎn)生中占有很大的比重。

圖10 濾波前后角度和角速度波形Fig.10 Angle and angular velocity before and after filtering

圖11 濾波后的角度、角速度與控制量之間的波形Fig.11 Waveform of angle，angular velocity and control quantity after filtering

4 結(jié)束語

本文從理論和實際2個方面對自平衡控制系統(tǒng)的穩(wěn)定性進行分析，依據(jù)理論和實際的分析結(jié)果，推導出維持系統(tǒng)保持穩(wěn)定性所需的條件。設計了自平衡控制系統(tǒng)測試平臺，在測試平臺上進行算法和改進控制策略的驗證。通過測試，系統(tǒng)能很好地完成傾角數(shù)據(jù)的處理，調(diào)節(jié)系統(tǒng)處于平衡狀態(tài)，同時，將系統(tǒng)獲得的角度、角速度及相關的控制量數(shù)據(jù)實時傳送到上位機，生成波形圖，方便了解各參量之間的關系。

［1］屠運武，徐俊艷，張培仁，等.自平衡控制系統(tǒng)的建模與仿真［J］.系統(tǒng)仿真學報，2004，16(4):839-841.TU Yunwu，XU Junyan，ZHANG Peiren，et al.Model and Simulation of Self-balance Control System［J］.Jour-nal of system simulation，2004，16(4):839-841.

［2］肖樂.兩輪自平衡機器人建模及智能控制研究［D］.哈爾濱:哈爾濱理工大學，2011:5-14.XIAO Le.Two-wheel Self-balancing Robot Modeling and Intelligent Control［D］.Harbin:Harbin University of Science and Technology，2011:5-14.

［3］孫建勤.兩輪自平衡小車大范圍鎮(zhèn)定方法研究［D］.西安:西安電子科技大學，2010:34-54.SUN Jianqin.Research on Methods of the Two-wheeled Self-balancing Vehicle’s Wide-range Stability［D］.Xi an:Xidian University，2010:34-54.

［4］YONG T K，BIEN Zeungnam.Robust self-learning fuzzy controller design for a class of nonlinear MIMO systems［J］.Fuzzy Sets and Systems，2000，111(2):117-135.

［5］HYUNGJIK L，SEUL J.Balancing and navigation control of a mobile inverted pendulum robot using sensor fusion of low cost sensors［J］.Mechatronics，2012(2):95-105.

［6］SHIROMA N，MATSUMOTO O，KAJITA S，et al.Cooperative behavior of a wheeled inverted pendulum for object transportation［C］//Proceedings of the 1996 IEEE/RSJ International Conference on.Intelligent Robots and Systems.Japan，Osaka:IEEE Press，1996，2:396-401.

［7］胡壽松.自動控制原理［M］.4版.北京:科學出版社，2007.HU Shousong.Automatic control theory［M］.4th Edition.Beijing:Science Press，2007.

［8］SPONG M W，CORKE P，LOZANO R.Nonlinear control of the reaction wheel pendulum［J］.Automatica，2001，37(11):1845-1851.