兩輪自平衡機器人動力學建模及其平衡控制
2009-01-01 00:00:00阮曉鋼任紅格
計算機應用研究 2009年1期
（北京工業(yè)大學 電子信息與控制工程學院， 北京 100124）
摘 要：針對高階次、不穩(wěn)定、多變量、非線性、強耦合的兩輪自平衡移動機器人系統(tǒng)，采用Lagrange方程推導出動力學模型，對其進行穩(wěn)定性和可控性判斷，并利用LQR和龍伯格極點配置的方法在此模型的基礎上對兩輪自平衡機器人的姿態(tài)和速度進行控制，可獲得較為穩(wěn)定的動態(tài)平衡過程。給出了數(shù)學模型推導的具體步驟，分別采用以上兩種方法進行了仿真研究和比較。仿真實驗結果表明，這兩種控制方法對機器人的穩(wěn)定性控制都是有效的。其中龍伯格極點配置控制方法使系統(tǒng)的跟蹤速度更快、穩(wěn)定性更高，具有較高的實際應用價值。
關鍵詞：兩輪自平衡移動機器人； 動力學模型； 線性二次型調節(jié)器； 龍伯格極點配置； 動態(tài)平衡控制
中圖分類號：TP391 文獻標志碼：A
文章編號：10013695(2009)01009903
Twowheeled selfbalancing mobile robot dynamic model and balancing control
RUAN Xiaogang， REN Hongge
(School of Electronic Control Engineering， Beijing University of Technology， Beijing 100124， China)
Abstract:Aiming to the twowheeled selfbalancing mobile robot system with highrank， unstable， multivariable， strongly coupling， complicated dynamic nonlinear property，this paper established the dynamic model applied Lagrange program， and carried on the stability and the controllability judgment to it. Based on this model， using LQR and the Dragon Bergh extreme disposition method carried on the control to the twowheeled selfbalancing mobile robot in the posture and the speed， and obtained the stable dynamical balance process. Listed some material steps at establishing the mathematics model， and used above two methods to do the simulation research and the comparison separately. It indicates through the simulation experiment that the two control method to the robot stable control all are effective. Dragon Bergh extreme disposition control method causes the system that the track speed to be quicker， the stability is higher， and it has the high practical application value.
Key words：twowheeled selfbalancing mobile robot; dynamics model; LQR; Dragon Bergh extreme disposition; dynamical balance control
兩輪自平衡機器人系統(tǒng)是一個高階次、不穩(wěn)定、多變量、非線性、強耦合的系統(tǒng)，它實際上是一個可以行走的一級倒立擺。兩輪自平衡機器人越來越多地引起了國內外學者的廣泛關注，由于該系統(tǒng)為欠驅動系統(tǒng)，運動學方程不能完整地描述系統(tǒng)的行為，并且此時系統(tǒng)為不可控，要解決兩輪自平衡機器人系統(tǒng)的平衡問題就必須考慮動力學對它的影響。Ha等人[1]在系統(tǒng)的線性化動力學方程中較早地進行了位置跟蹤控制器的研究；Grasser等人[2]采用牛頓法推導出了系統(tǒng)的動力學方程，并在計算點附近對模型進行線性化，設計了控制器；Salerno等人[3]根據(jù)倒立擺特性，以兩輪機器人的旋轉角度為變量得到了系統(tǒng)的動力學方程，通過應用微分幾何方法對系統(tǒng)各種控制器參數(shù)的狀態(tài)變量進行了分析。
兩輪機器人在靜止狀態(tài)下不能穩(wěn)定平衡，若要其移動必須采用動態(tài)平衡[4]。機器人的平衡是一個動態(tài)過程，機器人在平衡點附近不停地變化進行調節(jié)以保持平衡。通常采用Lagrange方程的方法推導出系統(tǒng)的動力學方程，從而對機器人的位置和方向進行控制，該動力學方程可以排除非完整約束力的限制。因為動力學分析的方法是從總體能量的角度考慮的，在模型的建立中不用考慮系統(tǒng)內部之間的作用力，通過選用不同的狀態(tài)變量可以達到不同的控制目的。
本文采用Lagrange方法建立了兩輪自平衡機器人的多輸入、多輸出的非線性動力學模型，并進行了線性化處理；利用LQR、place極點配置和龍伯格極點配置的方法在此模型的基礎上對兩輪自平衡機器人的姿態(tài)和速度進行控制，且都獲得了較為穩(wěn)定的動態(tài)平衡過程，驗證了控制算法的有效性。
1 兩輪自平衡移動機器人動力學模型
1.1 兩輪自平衡機器人的結構特點
兩輪自平衡機器人是基于典型的移動倒立擺模型設計的，它僅有兩個輪子，以雙輪差速方式布置，每個輪子由直流電機通過減速器直接驅動，以電機軸心線為中心前后轉動。系統(tǒng)結構如圖1所示。
根據(jù)運動特性可將機器人分為兩個主要部分：a)機械系統(tǒng)。包括兩輪、減速機構、底盤、高速轉子、本體、框架等，負責實現(xiàn)機器人的機構功能、承載硬件電路、搭建工作平臺；b)控制系統(tǒng)。包括電機、驅動器、傳感器、電池、電源轉換電路以及DSP控制板等。其中傳感器為系統(tǒng)提供反饋信號，構造閉環(huán)系統(tǒng)的狀態(tài)空間方程，通過設計有效的控制算法來控制系統(tǒng)的平衡運動。
1.2 兩輪自平衡機器人的動力學建模及其參數(shù)說明
采用Lagrange方程進行系統(tǒng)動力學建模[5]，其數(shù)學表達式如下：
d/dt(T/k)-(T/qk)=Qk(k=1，2，…)(1)
其中：T為系統(tǒng)的總動能，qk為系統(tǒng)的廣義坐標，Qk為廣義力。
T=Jθ2/2+(m/2)[(R2(θl-θr)2L2sin2θ)/4f 2+
(-L cos θ-R(l+r)/2)2+2L2 sin2 θ]+
mlR22l/2+mrR22r/2+Jl2l/2+Jr2r/2+
(ml+mr)R2(l-r)2/8+(JR2(l-r)2/2)/4f 2(2)
系統(tǒng)的三個廣義坐標qk為θl、θr和θ；廣義坐標下的系統(tǒng)廣義力Qk為左輪轉矩Qθl、右輪轉矩Qθr和車體作用在x軸的轉矩Qθ。其中：
Qθl=Ml=Km(Ul-Kel)/Ra=0.032 8 Ul-0.010 9 θl
Qθr=Mr=Km(Ur-Ker)/Ra=0.032 8 Ur-0.010 9 θr
Qθ=mg L sin θ-Ml-Mr=9.8 sin θ-0.032 8(Ul+Ur)-0.010 9(l+r)(3)
將式(2)(3)代入式(1)中可得
d/dt(T/l)-(T/θl)=0.032 8Ul-0.010 9θl
d/dt(T/r)-(T/θr)=0.032 8Ur-0.010 9θr
d/dt(T/)-(T/θ)=
9.8 sin θ-0.032 8(Ul+Ur)-0.010 9(l+r)(4)
根據(jù)以上系統(tǒng)的動力學方程選擇狀態(tài)變量為x=(l，r，，θ)T，并進行線性化處理，即對于平衡點附近｜θ≤5°｜時，令sin θ=θ，cos θ=1，整理上式便可得到系統(tǒng)的狀態(tài)方程為
lr=
0.0195-0.22930-65.2850-0.2293-0.01950-65.28500.34500.34500244.89120010 lrθ+
-0..05880.69000.6900-0..0588-1.0381-1.038100
UlUr
y=-0.0750000-0.0750000000001 lrθ+0 00 00 00 0 UlUr(5)
該機器人模型中的參數(shù)是根據(jù)歐鵬公司生產的兩輪自平衡機器人的實際模型測量和計算得到的，參數(shù)如表1所示。
2 機器人系統(tǒng)的性能分析
2.1 兩輪自平衡機器人的能控能觀性判別
能控性和能觀性是控制器設計的前提。能控性是分析輸入對狀態(tài)的控制能力，是極點可實現(xiàn)任意配置的充分必要條件；能觀性是表征輸出對狀態(tài)的反映能力，故在設計前進行系統(tǒng)的能控能觀性分析[6]。
表1 機器人系統(tǒng)參數(shù)表
符號名稱單位數(shù)值符號名稱單位數(shù)值
ml左車輪質量kg0.8Jr右車輪轉動慣量kg#8226;m2Jw=0.001 8
mr右車輪質量kg0.8J繞x軸的轉動慣量kg#8226;m20.1
M車體質量kg10Jz繞z軸的轉動慣量kg#8226;m20
L車體質心到X軸的距離m0.1Ra電機內阻Ω1.11
f兩車輪軸距之一半m0.225Ke反電動勢系數(shù)v s/rad0.332 3
R車輪半徑m0.075Km力矩系數(shù)N m/A0.036 4
Jl左車輪轉動慣量kg#8226;m2Jl=Jrg重力加速度m/s29.8
由能控性矩陣M=[B，AB，A2B，A3B]=
可知，rank(M)=4，系統(tǒng)可控。
同理，由能觀性矩陣N=[C，CA，CA2，CA3]T可知，rank(N)=4，系統(tǒng)能觀。
2.2 兩輪自平衡機器人的穩(wěn)定性判別
穩(wěn)定性是自動控制系統(tǒng)中最重要的特性，因為一個不穩(wěn)定的系統(tǒng)是無法完成預期的控制任務的。系統(tǒng)的穩(wěn)定性是指系統(tǒng)在遭受外界擾動偏離原來的平衡狀態(tài)，而在擾動消失后，系統(tǒng)自身仍有能力恢復到原來平衡狀態(tài)的一種“頑癥”。 根據(jù)系統(tǒng)的特征值：
λ1=-15.741 4，λ2=15.557 5，λ3=-0.025 8，λ4=0.248 9
可由Lyapunov第一法進行穩(wěn)定性判斷。由于系統(tǒng)特征值存在正實根，在平衡點附近系統(tǒng)是一本質不穩(wěn)定系統(tǒng)，且其響應特性不太好，需采用各種控制器進行控制以達到預期控制目標。
3 機器人系統(tǒng)的控制器設計
兩輪自平衡移動機器人的運動控制最終表現(xiàn)為對其移動速度和轉動角速度的控制?？刂颇繕耸峭ㄟ^電機控制兩個獨立輪，使機器人按指定的移動速度和轉動速度運動，并且保持車體和擺桿平衡。這實際上是一個信號跟蹤問題[2，7]。
3.1 LQR控制器的設計
為了使兩輪自平衡機器人的平衡性能良好及控制能量少，且使系統(tǒng)達到無差跟蹤的效果，本文采用線性二次型最優(yōu)跟蹤控制器（LQR）[8]。
對上述所描述的機器人模型=Ax+Bu，y=Cx，給定對于狀態(tài)和控制的二次型性能指標函數(shù)
J=（1/2）∫∞0(xTQx+uTRu)dt
其中:Q是正半定對稱陣，R是正定對稱陣，這里選取Q=[1000，0，0，0;0，0，0，0;0，0，100，0;0，0，0，0]，R=eye(2) 。所謂LQR最優(yōu)控制問題，就是尋找一個控制量u*(#8226;），在保證性能指標J最小的同時使系統(tǒng)接近于平衡狀態(tài)，并使輸出跟蹤輸入。上述中，u*(t)=-K*x*(t)，K*=R-1BTP，且P是矩陣Riccati代數(shù)方程PA+ATP+Q-PBR-1BTP=0的解陣。由于解Riccati方程比較困難，借助MATLAB函數(shù)[K，P，E]=lqr(A，B，Q，R)來解，可得到最優(yōu)狀態(tài)反饋矩陣K*。
3.2 龍伯格極點配置控制算法的實現(xiàn)
龍伯格規(guī)范性狀態(tài)反饋陣相對于其他反饋矩陣求解方法惟一，能夠保證矩陣各元素的絕對值最小，且使系統(tǒng)具有良好的動態(tài)響應。本文采用龍伯格極點配置算法來計算狀態(tài)反饋矩陣K，以實現(xiàn)對機器人系統(tǒng)的穩(wěn)定性控制[6]。
通過對系統(tǒng)的性能分析可知，系統(tǒng)為完全能控的多輸入多輸出連續(xù)時間線性時不變系統(tǒng)=Ax+Bu，y=Cx。由A=S-1AS，B=S-1B，C=CS可導出其龍伯格規(guī)范形式為
=0.000 01.000 00.000 0-0.000 0
-0.000 00.000 01.000 0-0.000 0
6.325 4244.891 2-0.209 80.000 0
33.620 1-0.000 00.000 00.248 9×x+
-0.000 0-0.000 00.000 00.000 01.000 01.000 00.000 01.000 0
×u
y=-6.160 10.012 90.004 4-0.056 27.587 7-0.012 9-0.051 80.056 20000-0.000 0-1.038 10.000 0-0.000 0×x
其中龍伯格能控規(guī)范形的變換矩陣為
S-1=-0.052 5-0.052 5-0.031 9-0.000 0
-0.000 0-0.000 0-0.000 0-0.963 3-0.000 0-0.000 0-0.963 3-0.000 0
7.097 65.762 23.428 1-0.220 9
選取合適的系統(tǒng)的期望閉環(huán)極點P*，便可得到系統(tǒng)的龍伯格規(guī)范形狀態(tài)反饋矩陣K。其中K=KS-1 。
4 仿真結果及分析
為了驗證以上兩種控制算法的有效性，對系統(tǒng)做了相應的仿真實驗并進行了比較。在LQR控制器的設計中，巧妙地選取了Q和R，得到最優(yōu)狀態(tài)反饋矩陣K*，使控制器具有了良好的控制性能。其中：
K*=-18.470 1-0.272 2-22.015 2-269.340 525.906 2-0.522 9-11.568 8-200.890 2
當靜止時給機器人一個擾動，即x0=[0，0，0，0.2]，得到系統(tǒng)的仿真響應曲線如圖2所示。
在龍伯格極點配置的算法中，設系統(tǒng)的期望閉環(huán)極點為P*=[0.7+0.8i，-0.7-0.8i，-8，-56]，可得系統(tǒng)的龍伯格規(guī)范形狀態(tài)反饋矩陣
K=KS-1=-400.040 1-324.927 2-202.167 7-235.346 3399.232 8324.120 0192.824 3-12.424 8
當靜止時也給機器人一個x0=[0，0，0，0.2]的擾動，即得到系統(tǒng)的仿真響應曲線如圖3所示。
仿真結果表明，當機器人在靜止時加入一個擾動，LQR控制和龍伯格極點配置控制均能較好地控制系統(tǒng)的穩(wěn)定性，最終使機器人處于穩(wěn)定狀態(tài)。兩種控制方法相比較而言，LQR控制雖然比龍伯格極點配置控制的超調量小、控制方法簡單，但在系統(tǒng)跟蹤速度上卻慢得多，穩(wěn)定性也較差。所以龍伯格極點配置控制在實際中具有更高的應用價值。
5 結束語
本文研究兩輪自平衡移動機器人在平衡中的運動控制問題，利用Lagrange方法建立了系統(tǒng)的動力學模型，對其進行了性能分析。通過LQR控制方法和龍伯格極點配置控制算法，設計了控制器并做了相應的控制和仿真，結果表明這兩種方法都能使兩輪自平衡機器人保持平衡，達到良好的控制效果。相比較而言，龍伯格極點配置控制算法在系統(tǒng)跟蹤速度和穩(wěn)定性上都優(yōu)于LQR控制方法，具有更高的實用性。
參考文獻：
［1］HA Y S， YUTA S. Trajectory tracking control for navigation of the inverse pendulum type selfcontained mobile robot[J]. Robotics and Autonomous Systems，1996，17(12):6580.
[2]GRASSER F， D’ARRIGO A， COLOMBI S， et al. Joe:a mobile， inverted pendulum[J]. IEEE Trans on Industrial Electronics，2002，49(1):107114.
[3]SALERNO A， ANGELES J. On the nonlinear controllability of a quasiholonomic mobile robot[C]//Proc ofIEEE International Conference on Robotics and Automation. 2003:33793384.
[4]屠運武，徐俊艷，張培仁，等.自平衡控制系統(tǒng)的建模與仿真[J].系統(tǒng)仿真學報，2004，16(4)：839841.
[5]孔祥宣，曹其新.自主式雙輪動態(tài)移動機器人的控制系統(tǒng)研究[D]. 上海：上海交通大學，2007:2635.
[6]鄭大鐘.線性系統(tǒng)理論[M]. 清華大學出版社.2005:135223.
[7]楊興明，丁學明，張培仁，等.兩輪移動式倒立擺的運動控制[J].合肥工業(yè)大學學報， 2005，11，28(11)：14851488.
[8]胡躍明.非線性控制系統(tǒng)理論與應用[M].北京：國防工業(yè)出版社， 2002.
