龍 周，湯健華，江 勵，黃 輝，陳榮滿

（五邑大學智能制造學部，廣東江門529020）

0 引言

兩輪自平衡小車是一個欠驅(qū)動、高耦合、強非線性的運動系統(tǒng)，類似倒立擺的原理，可以通過檢測車體傾角，實時改變驅(qū)動兩輪的力矩，來實現(xiàn)車體的平衡。因其緊湊的機械結(jié)構(gòu)設(shè)計，在狹窄區(qū)域工作的靈活性和多功能性，以及可作為測試非線性控制算法的移動平臺和解決工業(yè)和社會中的一些問題，引起了人們的廣泛關(guān)注和重點研究，目前已經(jīng)廣泛應(yīng)用于物流機器人如波士頓公司的HANDLE，載人移動的如小米平衡車，機器人輪椅如Genny Mobility等。雖然兩輪自平衡小車應(yīng)用廣泛，但其核心問題仍是如何控制機器人保持平衡和實現(xiàn)穩(wěn)定運動，PID算法[1-2]作為應(yīng)用廣泛、效果優(yōu)良的經(jīng)典控制算法，LQR等狀態(tài)反饋控制算法[3-5]作為最優(yōu)控制算法，及上述算法的結(jié)合優(yōu)化相關(guān)參數(shù)[6]等都被廣泛應(yīng)用于自平衡小車的運動控制方案中，并都取得不錯的控制效果。智能控制與經(jīng)典控制理論相結(jié)合的模糊PID[7-9]，模糊自適應(yīng)算法[10]，以及相關(guān)智能算法[11-12]也都廣泛應(yīng)用于自平衡小車的控制中。

智能算法雖然能夠取得優(yōu)異的控制效果，且對兩輪自平衡小車的動力學模型的準確性要求較低，但存在參數(shù)選取困難，算法理解及設(shè)計復雜等問題，對算法的設(shè)計與實現(xiàn)帶來很大不便；經(jīng)典及現(xiàn)代控制理論中的算法理解相對容易，且基本能夠?qū)崿F(xiàn)控制目標和具有良好的控制性能，但其大多是基于線性化理論的基礎(chǔ)，對兩輪自平衡小車的建模有很高的要求，需要較為精確的數(shù)學模型才能設(shè)計出合理的控制器。

考慮到系統(tǒng)在建模時總會忽略掉一些諸如摩擦等因素,Balazs A等[1]使用PID控制，但對自平衡小車未建模以及忽略了傳動中摩擦的影響，導致自平衡小車在有界范圍內(nèi)出現(xiàn)有界振蕩。目前，對兩輪自平衡小車的許多動力學模型推導中都有一些常見的錯誤，這主要是由于對坐標系的誤解，而基于此解釋和描述機器人的運動，以及缺乏嚴格對比性[13]?；诖?，本文的主要目的是使用一種基于拉格朗日乘子的建模方法，在拉格朗日方程加入拉格朗日乘子，充分考慮被忽略掉的相關(guān)因素對系統(tǒng)的影響，并在該模型下設(shè)計一個對數(shù)學模型準確性要求較高的基于LQR算法的自平衡小車控制器，并通過MATLAB/Simulink及ADAMS的仿真實驗結(jié)果數(shù)據(jù)對比驗證了建模的精確性以及算法的有效性，以及系統(tǒng)抗干擾性和運動穩(wěn)定性。

1 系統(tǒng)建模及分析

1.1 系統(tǒng)模型

兩輪自平衡小車系統(tǒng)廣義上主要由車體與車輪組成，其SolidWorks模型如圖1所示，車體主要由控制電路板、電源、電機、驅(qū)動器以及連接和固定他們的剛性支撐板組成，車輪主要由左右車輪及轉(zhuǎn)軸組成，各部件均保證對稱安裝的原則，確保在平衡位置下系統(tǒng)的中心位于車輪轉(zhuǎn)軸之上。在運動過程，車體由于其重心的作用能夠繞車輪轉(zhuǎn)軸旋轉(zhuǎn)，其旋轉(zhuǎn)角度可以控制輪轂電機的驅(qū)動力來改變，從而使車體保持平衡。

圖1 SolidWorks中的系統(tǒng)模型圖

雖然系統(tǒng)組成相對簡單，但由于兩輪自平衡小車運動的復雜性，做合理的假設(shè)便利于數(shù)學模型的建立，本文基于倒立擺的模型，假設(shè)自平衡小車系統(tǒng)在運動過程中：（1）輪子與地面、輪軸與車體無相對滑動；（2）兩輪自平衡小車系統(tǒng)的重心位于兩輪輪軸中心上；（3）輪軸摩擦的能量損耗。則系統(tǒng)物理模型參數(shù)表達式含義如表1所示。

表1 系統(tǒng)模型參數(shù)

考慮兩輪自平衡小車的運動過程，以絕對坐標系OXYZ作為參考坐標系，輪軸系OXWYWZW以及車體系OXCYCZC隨小車移動定義3個坐標系，建立的簡化模型如圖2所示。圖中，兩輪自平衡小車在兩輪驅(qū)動力矩τL、τR的作用下，假設(shè)輪軸中心由點A運動到點B，則兩輪自平衡小車運動到點B后位置和姿態(tài)可由一組描述不同參數(shù)的變量唯一確定，這組變量可定義為：輪軸系的原點在絕對坐標系下的坐標由（X0，Y0）唯一確定，輪軸系的姿態(tài)則由兩輪的旋轉(zhuǎn)角位移θRL、θRR唯一確定，車體系的姿態(tài)由車體繞Y軸旋轉(zhuǎn)角度θ，車體繞Z軸旋轉(zhuǎn)角度φ唯一確定。

圖2 點A運動到點B的坐標變換及狀態(tài)變量變化

1.2 推導動力學方程

根據(jù)圖2所示，以絕對坐標系為參考系，輪軸系的質(zhì)心位移XR，角位移θR，繞Z軸旋轉(zhuǎn)角度φ可分別由以下公式導出：

由于車體系在運動過程中參考車輪系僅有旋轉(zhuǎn)運動，故兩者繞Z軸的旋轉(zhuǎn)角度相等，其質(zhì)心P1的位移在X軸分量XRM，Z軸分量ZRM以及繞Y旋轉(zhuǎn)角度θP可分別由以下公式導出：

由式（1）~（6）可知，當XR、θ、φ已知時，其他變量也可求，且數(shù)量上跟兩輪自平衡小車系統(tǒng)的自由度數(shù)是相等的，于是，在絕對坐標系中，描述兩輪自平衡小車的位姿，選取合適的狀態(tài)量，可由輪軸系質(zhì)心位移XR、俯仰角θ、偏航角φ作為系統(tǒng)廣義坐標唯一確定。

根據(jù)以上確定的系統(tǒng)廣義坐標，計算系統(tǒng)的總動能，利用拉格朗日方程方程建立數(shù)學模型，拉格朗日方程的表達式及各參數(shù)定義如下：

式中：L為拉格朗日函數(shù)，等于系統(tǒng)的總動能T減總勢能U；qi為系統(tǒng)的狀態(tài)變量，q=(XR,θ?c,φ,θ,X?R,θ?c,φ?,θ?)；Fi為系統(tǒng)的廣義力,F=（τL+τR，τL+τR，τL-τR，0）。

車輪的動能TW由兩車輪在X方向上的平動動能及繞Y、Z軸的轉(zhuǎn)動動能組成，推導公式如下：

車體的動能TC由在X方向上的動能及繞Y、Z軸的轉(zhuǎn)動動能組成，其中將繞Z軸旋轉(zhuǎn)角分解為繞YC、ZC兩個方向的運動，推導公式如下：

于是系統(tǒng)的總動能可由如下公式計算：

以XY所在平面作為零勢面，系統(tǒng)的總勢能：

考慮到系統(tǒng)以及摩擦力的復雜性，以及在模型簡化中忽略的各處的摩擦和阻力，便在拉格朗日函數(shù)之中加入拉格朗日乘子函數(shù)，以充分考慮摩擦力等因素對系統(tǒng)的影響，以便提高所建立的的數(shù)學模型的準確性。

定義拉格朗日乘子：

加入拉格朗日乘子的拉格朗日函數(shù)：

而不加拉格朗日乘子，并將公式中的X?R用Rθ?c替換，得到的拉格朗日函數(shù):

所以根據(jù)拉格朗日方程，分別對式（13）～（14）計算得到的拉格朗日函數(shù)，代入式（15）計算，由于式（14）中所有X?R用Rθ?c替換了，故不進行有關(guān)X?R項的求解。

由式（13）推導出加入拉格朗日乘子簡化后的兩輪自平衡小車的非線性動力學方程：

由式（14）推導出不加拉格朗日乘子簡化后的兩輪自平衡小車的非線性動力學方程：

式（16）中：

1.3 動力學模型驗證

對式（15）～（16）的隱式微分方程組，可由公式q?=M-1(q)F(q,q?)轉(zhuǎn)換為顯示微分方程組，再借用MATLAB中的ode45函數(shù)進行微分求解?？紤]到系統(tǒng)只具有3個自由度，式（15）在轉(zhuǎn)換為顯示微分方程組之后，去掉其中一條之前由拉格朗日乘子引入的有關(guān)λ的微分方程，以保證和式（16）中的未知變量和數(shù)目一致，于是，代入表2所示系統(tǒng)各參數(shù)的值，求解得到的關(guān)于變量在是否有加入拉格朗日乘子函數(shù)的各變量隨時間變化的比較圖，如圖3所示。

表2 參數(shù)數(shù)值列表

圖3 Tl=Tr=0.1時數(shù)據(jù)對比圖

為了驗證所建立的數(shù)學模型的準確性，基于表2中的參數(shù)數(shù)值，在ADAMS中搭建的簡化的兩輪自平衡小車模型。在相同輸入，相同的初始條件下，兩輪自平衡小車的相關(guān)狀態(tài)變量隨時間的變化如圖3所示，并將其與相同條件下MATLAB的求解數(shù)據(jù)進行比較。

由圖可知，基于數(shù)學模型中加入拉格朗日乘子的數(shù)據(jù)曲線與物理模型的系統(tǒng)狀態(tài)量的響應(yīng)曲線圖在短時間內(nèi)幾乎一致。但隨著時間的延長，相較于數(shù)學模型，物理模型各狀態(tài)量的曲線其周期和幅值均有一定量的衰減?？紤]到物理模型更加接近于實際情況，并在輪子與地面加入了庫倫摩擦等耗能因素，而且兩者的求解器以及求解步長的不同，當ADAMS求解步長設(shè)置合適的步數(shù)時，其結(jié)果曲線更加精確和更接近MATLAB的數(shù)據(jù)曲線，所以，這并不影響使用上面建立數(shù)學模型來研究兩輪自平衡小車系統(tǒng)。

2 控制算法設(shè)計

2.1 建立狀態(tài)空間模型

由于式（14）中的動力學方程是非線性的，為了進一步研究，需要在平衡點附近對方程進行線性化處理，在小角度范圍內(nèi)，可近似考慮：sinθ≈θ，cosθ≈1，并且忽略所有的導數(shù)項的高階項。由式（15）推導得到的狀態(tài)空間表達式如下：

其中：

2.2 能控性分析

根據(jù)上小節(jié)建立的狀態(tài)空間方程，借用MATLAB中的eig函數(shù)求解系統(tǒng)矩陣A的特征值得：eig(A)=[-1.175 8 1.175 8-0.000 010.000 01 0 0]T，可知兩輪自平衡小車系統(tǒng)存在2個正實根，是一個不穩(wěn)定的系統(tǒng)，但是rank（B，AB，A2B，…，A5B）=6，說明其能控判別矩陣滿秩，系統(tǒng)可以設(shè)計控制器使其保持穩(wěn)定。

2.3 LQR算法設(shè)計

本文基于LQR算法，對兩輪自平衡小車的相關(guān)狀態(tài)變量及其微分引入狀態(tài)反饋，完成對兩輪自平衡小車傾角及位置的控制。LQR最優(yōu)算法的數(shù)學意義是通過尋找一個狀態(tài)反饋控制矩陣K，使系統(tǒng)的具有良好的跟蹤性能，并使二次型性能指標函數(shù)J取得最小值[6]。

式中：Q和R分別為正定的相關(guān)狀態(tài)變量權(quán)矩陣和正定的控制輸入量權(quán)矩陣。

當Q陣中對應(yīng)元素的權(quán)值增加時，該元素對應(yīng)的狀態(tài)量的變化對系統(tǒng)的影響增大，雖然這會加快系統(tǒng)的調(diào)整時間和達到平衡的時間，但也會使系統(tǒng)產(chǎn)生相應(yīng)的振蕩，所以Q、R取值決定著系統(tǒng)動態(tài)性能。在經(jīng)過多次仿真實驗，比較不同權(quán)重下的仿真效果，得出Q在對角線上取值依次為Q11=2，Q11=1，Q11=1，Q11=4，Q11=2，Q11=1下，系統(tǒng)具有更優(yōu)的控制效果，在此基礎(chǔ)上，再借用MATLAB函數(shù)的LQR函數(shù)求解得到反饋控制器K陣：

此時系統(tǒng)的特征值為：

由上可知：特征值都位于負半平面且落在虛軸附近，說明系統(tǒng)在該控制器下處于穩(wěn)定且震蕩較小。

3 仿真實驗結(jié)果對比分析

本節(jié)基于第2節(jié)建立的狀態(tài)空間方程在MATLAB/Simulink搭建如仿真圖，如圖4（左）所示。加入第2節(jié)計算得到的狀態(tài)反饋K陣，得到的仿真結(jié)果。然后在ADAMS中搭建簡化的兩輪自平衡小車的虛擬樣機的物理模型如圖4（右）所示，根據(jù)表2中的數(shù)值設(shè)置兩輪自平衡小車的參數(shù)，使用控制工具包設(shè)計并實現(xiàn)LQR控制算法，得到虛擬樣機的仿真結(jié)果，并將該結(jié)果與Simulink的結(jié)果數(shù)據(jù)對比并分析。

圖4 不同環(huán)境下的仿真模型

3.1 仿真模塊搭建

圖4 所示為在Simulink中搭建的基于動力學方程仿真模型（左）和基于實際模型簡化的ADAMS中的物理模型（右）。

由圖可知當不需要執(zhí)行運動指令時，左右輪輸入力矩為0，此時兩輪自平衡小車系統(tǒng)處于自治平衡狀態(tài)，系統(tǒng)檢測到車體角度誤差，經(jīng)控制器K會反饋回一個力矩值，驅(qū)動自平衡小車作出反應(yīng)保證車體角度誤差重新為零，當左右輪輸入不同的驅(qū)動力矩，會驅(qū)動自平衡小車作出不同的運動行為，輸入力矩值相等時的直行運動或值不相等時旋轉(zhuǎn)運動。而采用兩種不同軟件分別獨立的對兩輪自平衡小車系統(tǒng)進行控制仿真分析，且兩個環(huán)境下的參數(shù)設(shè)置互不干擾，但采用同一種控制算法進行實驗，可以更好地驗證控制算法的合理性及有效性。

3.2 仿真結(jié)果

（1）自平衡實驗

首先考慮簡單控制任務(wù)，使其能夠?qū)崿F(xiàn)基本平衡，并在初始狀態(tài)下給控制系統(tǒng)加上一個值為1的階躍信號，為了使系統(tǒng)更好地跟隨，對階躍加上一個預補償值-1.141 5的增益，在左右車輪初始輸入力矩Tl=Tr=0，得到的仿真結(jié)果如圖5所示。由圖可知，在上節(jié)所設(shè)計的控制算法下，基于ADAMS和MATLAB環(huán)境得到的系統(tǒng)狀態(tài)變量隨時間變化的數(shù)據(jù)曲線基本重合：零時刻加入一個值為1的階躍信號后，破壞原保持的平衡狀態(tài)，系統(tǒng)仍能快速作出反應(yīng)并迅速調(diào)整，兩輪自平衡小車傾角在5 s左右的時間內(nèi)回歸零值，并移動在距初始位置0.1 m處并保持穩(wěn)定。

圖5 自平衡實驗下狀態(tài)變量變化

（2）直行實驗

在直行實驗中，要求小車能夠直行4 m，且在過程中保持平衡。初始時刻給小車的左右輪一個隨時間變化，斜率為4，持續(xù)時間為10 s，且10 s后值為40且保持穩(wěn)定的斜坡信號，并也加上預補償值-1.141 5，得到的仿真結(jié)果如圖6所示。

圖6 4 m直行實驗狀態(tài)變量變化

由圖可知，自平衡小車在12.5 s左右的時間內(nèi)能準確的移動到4 m的位置并保持穩(wěn)定，由于系統(tǒng)欠驅(qū)動性和非線性的影響，小車在加減速過程中，也并沒有過大的變化而使自平衡小車最終脫離平衡位置。對比ADAMS與MATLAB的仿真的結(jié)果可知，兩個環(huán)境下的自平衡小車其各狀態(tài)量的變化曲線也基本重合。且通過多次實驗，通過改變輸入力矩斜坡信號的斜率，可以充分改變自平衡小車移動到目標點的時間以及勻速運動的時間和速度大小。

（3）旋轉(zhuǎn)實驗

在保持平衡的基礎(chǔ)上，考慮讓小車進行自轉(zhuǎn)，在原有階躍信號的基礎(chǔ)上，再對左右輪加上大小相等，值相反的力矩，使得左右車輪初始輸入力矩為一個隨時間變化，斜率為π/20，持續(xù)時間為5 s，且5 s后值為π/20且保持穩(wěn)定得斜坡信號，并也加上預補償值-1.141 5，得到的仿真結(jié)果如圖7所示。由圖可知，在左右輪輸入力矩不等時，小車將進行自旋運動，對比ADAMS與MAT?LAB的仿真結(jié)果，各狀態(tài)變量的變化值仍幾乎保持一致。對比自平衡實驗的位移與速度變化曲線，可知自平衡小車在自旋運動下，對初始階躍的響應(yīng)仍然有效，自平衡小車在13 s完成90°的自旋后仍可保持穩(wěn)定，與執(zhí)行勻速實驗一致，通過改變輸入力矩斜坡信號的斜率，可以充分改變自平衡小車旋轉(zhuǎn)到目標角度的時間以及可在過程中保持勻速旋轉(zhuǎn)。

圖7 旋轉(zhuǎn)實驗狀態(tài)變量變化

4 結(jié)束語

本文在建立兩輪自平衡小車系統(tǒng)的數(shù)學模型的過程中，通過對比是否加入拉格朗日乘子得到的狀態(tài)變量隨時間變化的數(shù)據(jù)曲線，以及在相同條件下，將這些數(shù)據(jù)與基于ADAMS的物理模型與基于MATLAB的數(shù)學模型得到的數(shù)據(jù)曲線相對比，由于ADAMS仿真與MATLAB仿真結(jié)果數(shù)據(jù)相互獨立，結(jié)果表明加入拉格朗日乘子的方式來推導兩輪自平衡小車動力學方程與ADAMS的仿真數(shù)據(jù)更加接近，從而證明了數(shù)學建模的精確性。

本文在準確建模的基礎(chǔ)上，在不同環(huán)境下設(shè)計同一套控制算法，結(jié)果表明：基于Simulink，ADAMS的相互獨立的仿真實驗都實現(xiàn)了該套LQR控制算法，且其仿真結(jié)果數(shù)據(jù)的曲線幾乎一致，并且實現(xiàn)了兩輪自平衡小車的基本運動功能，從而驗證了控制算法的有效性以及建模的精確性。