基于線性二次調節器算法的兩輪自平衡機器人最優控制方法

徐 鑫，劉 彬，劉東洋

(空軍工程大學信息與導航學院，西安 710077)

隨著科學技術的發展，機器人和自動化產業變得越來越普遍，并與日常生活融為一體。從智能家居到語音交互再到人機共存，人工智能無論是在效率還是在便利性方面都正朝著改善人們生活的方向發展。例如家庭服務的掃地機器人，公共服務的導游，迎賓機器人，工業生產的搬運、碼垛、包裝機器人，以及國防軍事、公共安全方面的特種機器人[1]。

兩輪自平衡機器人作為一類具有兩個獨立制動的車輪的自平衡機器人平臺，其重心位于車輪旋轉軸線上方，僅靠兩個輪子來實現平衡和靈活的移動，并且它可以幫助用戶攜帶物品運動而不使物品掉落，從而為用戶提供一雙“額外的手”。在兩個車輪上保持平衡是自平衡機器人的核心部分，為了優化這些機器人的平衡機制，開發一種機電控制系統使自平衡機器人能夠盡快平衡自身顯得尤為重要。

兩輪自平衡機器人的運動行為類似于經典的倒立擺機械系統，可以在垂直位置進行平衡。該系統具有固有的不穩定性，即使輕微的干擾也會導致系統跌落。因此，中外眾多專家學者提出了多種不同方案，也取得了重要成果[2-6]。楊興明等[7]分層滑模控制策略推導系統總的控制律，利用模糊邏輯的萬能逼近功能進行估計，進一步改善了控制系統的品質。張俊杰等[8]采用模糊自適應控制策略解決了兩輪自平衡機器人的模型多樣化的自適應運動控制問題。文獻[9-10]針對兩輪自平衡機器人的參數不確定性開展了基于滑模變結構跟蹤控制的研究。宋寅卯等[11]采用線性自抗擾控制解決了兩輪自平衡車因不同用戶身高體重差異導致系統控制穩定性能差的問題。這些研究取得了很重要的理論成果及應用。當然，自平衡機器人控制系統的研究還逐漸以產品的形式出現在人們的日常生活，例如Segway或Hoverboard平衡車。此外，該類機器人緊湊的結構和良好的機動性在更多方面可以幫助人們日常生活[10-11]，它們可以很大，可以在建筑工地周圍幫助工人運輸混凝土塊，它們也可以很小，在辦公室內為人們端茶送水、傳遞文件，例如助理機器人[12]、殘疾人輪椅[13]。

為了深入理解兩輪自平衡機器人的運動平衡機制并以此來設計控制器，從而實現對兩輪自平衡機器人的控制。首先，通過分析自平衡機器人的運動原理；其次，建立系統數學模型并在狀態空間實現中表示；此外，設計狀態估計反饋控制器使機器人穩定在不穩定的平衡點附近；最后，引入線性二次調節器(linear quadratic regulator，LQR)控制方法得到更好的控制效果。實驗結果表明，LQR控制方法對于這類自平衡機器人運動平衡控制具有良好的穩定性和魯棒性。

1 問題定義與解決方案

1.1 問題定義

倒立擺是控制系統中的經典問題，這類系統的不穩定性和非線性很適合用于探索不穩定的動力學。常見的例子如圖1所示。

影響雙輪自平衡機器人機體保持平衡和直立的原因可以分為軟件和硬件兩個方面，軟件方面的問題是所使用的慣性傳感器輸出噪聲大，需要對傳感器的原始輸出進行濾波處理，并使用控制理論方法加以控制，才能給控制器提供可靠的性能，并且防止階躍擾動。而硬件方面的問題是微控制器運行控制算法所需的計算時間。過量的計算延遲導致倒立擺傾斜角的修正時間較長，使系統偏離垂直平衡位置。與傳統的機器人相比，在自動平衡機器人的情況下，驅動電機保持平衡位置所需的功率要高數倍。

1.2 解決方案

在軟件方面，為了濾除慣性傳感器的噪聲輸出，需要使用濾波算法融合加速度計和陀螺儀的輸出，以提供可靠的傾斜角度信息。目前常見的濾波算法有卡爾曼濾波、互補濾波、粒子濾波等。良好的數字控制算法可以提供最佳控制比例積分微分(proportion integration differentiation，PID)和線性二次調節器(LQR)。其他研究工作也探索了線性-高斯控制(LQG)，模糊邏輯和極點配置方法的使用。

1.2.1 PID控制器

PID控制器是一種控制回路反饋，廣泛用于工業和需要連續調節的控制系統。控制器連續計算系統的誤差值e(t)，并通過比例、積分和微分項調節控制輸入量u(t)，以使其隨時間變化將誤差降到最小。在數學上，其控制函數可表示為

(1)

式(1)中：u(t)為控制器的輸出；Kp、Ki、Kd分別為非負比例、積分和微分系數。

1.2.2 LQR控制器

LQR控制集中于線性系統或線性化非線性系統以及成本函數的選擇其中成本函數是狀態和控制的二次函數。

通常情況下，線性系統線性系統由其狀態空間表達式給出，其中包括矩陣A、B和C，表達式為

(2)

尋找一個使成本函數J最小的控制函數u，即

(3)

式(3)中：Q為正定(或正半正定)對稱矩陣；R為正定對稱矩陣。兩個成本矩陣Q和R被稱為權重矩陣，以便在稱重狀態向量需要每個相應元件有多快達到其所需的最終值引入。

利用代數黎卡提方程計算增益并最小化成本函數，則有

(4)

式(4)中：K為增益矩陣；P為軸心的位置。

在硬件方面，芯片技術的飛速發展為微控制器提供了更通用、更高效、更便宜和更可靠的選擇。例如Arduino就是一款性能強、功耗低和成本低的開發板，濾波算法和控制算法都可以在Arduino中輕松實現。同時，配備高扭矩直流電機可以將機器人驅動至其垂直平衡位置并平衡整個物理結構。此外跟蹤速度也是至關重要的，因為速度信息在車輪編碼器的反饋回路中反饋給微控制器，以最大限度地減少誤差以達到平衡。

2 兩輪自平衡機器人數學模型

2.1 運動原理

通常而言，自平衡機器人平臺由兩個輪子連接在一個帶有馬達的車軸上。從車軸起，平臺將垂直延伸以能夠保持平衡。當平臺傾斜時，其上的力不再與車軸垂直方向對齊，在平臺的質心周圍產生一個力矩。因此，必須有一個扶正的時刻，防止機器人翻倒。電機將在車輪上施加力矩并在平臺上施加相等且相反的力矩。這種機制允許平臺在傾斜時保持垂直。如果平臺向前傾斜，那么輪子就會向前移動以保持它的直立。驅動車輪前進的時刻在平臺上產生了一個向后的力矩，從而使它恢復平衡。

2.2 兩輪自平衡機器人系統模型建立

系統數學模型建立是為物理結構設備形成一套有效控制策略的重要前提。為了推導兩輪自平衡機器人的數學模型，將車身建模為質量集中于重心且旋轉軸高于車輪軸的倒立擺，它將機器人限制為其功能的基本要素，以便將系統縮減為更簡單的形式，可以更輕松地處理而不會損失實際系統的準確性。在該系統中，機器人的電機驅動車輪從原點行進時的移動距離與車輪的角加速度直接相關，同時，期望在運動過程中車體保持角度為0 rad。因此，兩輪自平衡機器人的數學模型可以分為直流電機線性模型、車輪模型和擺體模型3個部分，最終得到系統的狀態空間模型。如圖2所示為兩輪自平衡機器人平面模型。通常認為兩輪自平衡機器人屬于倒立擺的范疇，因此在進行物理建模時，常采用類似倒立擺的模型，其平面模型與倒立擺平面模型不同的是，兩輪自平衡機器人還需要加以分析輪子的運動模型。

α為擺角；C為車軸位置；θ為車輪旋轉角度；L為車體軸心到車軸之間的距離；rw為車輪半徑

2.2.1 直流電機線性模型

機器人使用兩個直流電動機來驅動車輪，電動機的電氣等效模型如圖3所示。

V為電機電壓；R為電機電阻；La為銜鐵電感；i為銜鐵電流；kb為電機反電動勢常數；emf為反電動勢；kt為電機轉矩系數；Tm為電機扭矩；ξ為電機相對于定子的旋轉速度

電機模型需要將施加的電壓與傳遞的扭矩相關聯。該模型可以通過將電壓與電流關聯得到相關表達式為

(5)

電流與扭矩的關系為

(6)

式(6)中：Im為電機轉子慣量；bm為電機轉子的黏滯阻力。

假設電氣系統響應比機械響應快得多，則電感可以忽略。此外，電刷電壓和黏滯摩擦也都可以忽略不計。結合式(5)、式(6)，并注意到電樞旋轉角度是機器人主體角度α與車輪旋轉角度θ之差，扭矩可以表示為

(7)

2.2.2 車輪模型

電機驅動車輪，車輪將機器人連接到地面。圖4為車輪的平面運動模型。

Fp為擺體在車輪上的作用力；Fs為車輪與地面之間的反作用力；F為電機軸上的水平力；mw為車輪的質量；rw為車輪半徑；Tm為電機扭矩；Tf為與電機角速度成比例的摩擦轉矩

對繞車輪軸的扭矩求和以得出運動的旋轉方程，得

(8)

式(8)中：Icm,w為車輪質心的慣性矩。

式(8)可與電動機轉矩表達式組合以消除變量，得

(9)

車輪的運動平移方程為

(10)

2.2.3 擺體模型

針對水平方向，繞機器人質心的旋轉以及垂直于擺的運動，得出了圖5所示的機器人主體的運動方程為

P為擺體參考點；α為擺體與垂直方向的俯仰角；L為車輪中心到P點的距離；mp為擺體質量；Ip為P點的慣性矩；x為車輪的水平位移；C為車輪中心位置

(11)

(12)

(13)

假設車輪不打滑，x=rwθ，并且等價于電機軸上的力，可以將式(7)、式(12)和式(13)組合在一起，以消除電機軸上的水平力，得

(14)

上述方程式中具有非線性項。因此，使用小角度假設將系統圍繞所需的平衡點(擺角，α=0)進行線性化[其中sinα≈α,cosα≈1]，可得

(15)

最終多項公式整理可得

(16)

2.2.4 狀態空間模型

得到系統的線性化方程后需要將其轉換為狀態空間形式。對于物理設備機器人而言，系統的狀態變量x、系統輸入u和系統輸出y的定義為

(17)

使用模型中的線性化方程，可以將該模型的狀態空間表示為

A=

(18)

(19)

(20)

式中：

(21)

測量單個機器人的參數與模型推導分析同樣重要，系統模型中機器人的質量、擺的慣性常量等都會影響系統的穩定性和控制器的控制效果。因此，經過多次仿真測試與實體機器人測試，得到機器人的測量參數，如表1所示。

表1 兩輪自平衡機器人測量參數

3 系統控制器設計與仿真分析

閉環系統也稱為反饋系統，其使用系統之前的狀態及其模型來計算系統的輸入。這種類型的系統會檢查其輸出是否達到結果，是否存在任何錯誤。同時，該系統可以糾正錯誤，并且可以容忍系統中發生的任何干擾。對于兩輪自平衡機器人而言，其控制器效率將通過垂直平衡、位置控制和干擾抑制能力進行測試。

3.1 系統穩定性分析

對于穩定系統，可以利用能控能觀判別條件檢查系統的可控性和可觀察性。根據系統線性化空間狀態方程的A、B、C和D矩陣，其中D為四行一列的0矩陣，并使用MATLAB對系統的可控性和可觀性進行判斷，如圖6所示。

圖6 系統能控能觀性判斷結果

從系統的可控制性矩陣和可觀察性矩陣中發現兩種矩陣的秩為4，即都是滿秩的。因此，該系統既可控制，又可觀察。

3.2 控制器設計

3.2.1 狀態估計

使用擺的狀態空間模型中的A和C矩陣找到閉環狀態估計器。估計器的A矩陣計算公式為

Aest=Asys-LCsys

(22)

式(22)中：L為估計器內的反饋增益矩陣。

放置系統極點可以使系統穩定，并通過將極點遠離y軸放置在S平面的左半部分，從而找到增益L以使系統比原有系統速度快5～6倍。

利用系統狀態空間的A、B、C和D矩陣解得系統極點分布圖，如圖7所示。

圖7 系統極點分布圖

將系統極點重新放置在S平面左半部分，并代入估計器，得到系統輸出，如圖8所示。

從圖8可以看出，估計器狀態雖然完全遵循原始系統狀態，但是狀態從x軸向無窮大發散，因此系統仍然不穩定。

圖8 狀態估計控制器與輸出結果

3.2.2 反饋控制

系統估計器已經完美跟蹤狀態。現在需要通過設計反饋控制器來使系統穩定。新的A矩陣可以表示為

Actr=Asys-BsysK

利用狀態估計獲得的增益L，可以得到系統的極點為[-67.2，-59.2，-12，-6]，如圖9所示。

圖9 重新配置后的系統極點分布

將極點放置在[-67.2，-59.2，-12，-6]上，并通過在MATLAB中使用place命令找到了最佳增益K。將增益K代入到反饋控制器中，得到系統的輸出如圖10所示。

從圖10可以看出，系統輸出最初變化很大，但最終穩定到零。現在已限制輸出，并且系統已穩定。

圖10 反饋控制器與輸出結果

3.2.3 具有狀態估計的反饋控制

根據狀態估計和反饋控制的效果，可以確定設計具有狀態估計的反饋控制器會形成一個完整的穩定系統，如圖11所示。

圖11 狀態估計反饋控制器與輸出結果

從圖11可以看出，系統起初的誤差較大，但是隨著時間的增加而逐漸降低。此外，系統在狀態有界時變得穩定，并且穩定到零，同時估計器狀態完全遵循原始系統的狀態。

3.3 仿真實驗與結果分析

根據設計好的具有狀態估計的反饋控制器雖然可以保證系統穩定，達到較好的控制效果，但初始條件下的誤差較大，對于實際的兩輪自平衡機器人而言，很容易出現起擺失敗甚至跌倒的情況。因此，在最終的系統仿真中利用兩輪自平衡機器人的實際物理數據進行模擬仿真，同時引入LQR控制方法以期望取得更好的控制效果和控制成本。

從式(4)中可以知道除了Q和R以外的其余參數均為已知的，其中Q與控制器精度有關，R與控制器工作量有關。通常情況下，Q矩陣都是根據實際經驗估計得到，需要依照實際控制效果進行修改。

經過多次仿真后，確定矩陣Q和R為

利用式(4)使用MATLAB求解方程，可以得到控制器的極點位置和增益。而有了這些參數，機器人就可以保持平衡運動。利用MATLAB設計兩輪自平衡機器人運動動畫界面，其中可以看到機器人在10 s內的運動位置，擺體的旋轉角度以及擺體與垂直方向的狀態對比，如圖12所示。

從圖12可以看出，機器人在運動過程中的整個運動狀態分為加速、勻速、減速和保持平衡4個狀態。此外，機器人剛啟動時向前的加速度較大，擺體向前傾斜以平衡向后的反作用力，因此機器人擺體與垂直方向的角度偏差較大；當機器人運動到0.3 m處時，機器人從勻速狀態逐漸進入減速狀態，此時機器人加速度為負，從而導致機器人擺體稍微向后傾斜；當機器人接近并到達0.5 m處時，機器人進入保持平衡狀態，機器人擺體與垂直方向基本重合，偏差為-4.778×10-6rad。

圖12 兩輪自平衡機器人運動動畫

系統的響應速度與穩定性分析結果如圖13所示。從圖13可以看出，機器人在0～0.5 s的運行過程中擺體與垂直方向的角度偏差最大(約為 0.1 rad)，而且機器人啟動時的狀態類似于倒立擺起擺狀態(先向負方向回擺，后向正方向加速)，機器人進入加速階段；機器人在1～1.5 s的運行過程中擺體角度逐漸減小，位移曲線的斜率基本不變，說明機器人進入勻速階段；機器人在1.5～4.5 s的運行過程中，擺體角度逐漸趨于0 rad，位移曲線斜率變小，說明機器人進入減速階段；4.5～10 s的運行過程中，擺體角度基本保持在0 rad，位移基本保持在0.5 m，說明機器人進入保持平衡階段。此外，可以看出加入LQR控制方法可以有效地降低機器人啟動時的系統誤差，雖然延長了系統的穩定時間，但是可以避免出現擺體傾角過大而可能出現起擺失敗甚至跌到的情況。

圖13 階躍響應下LQR優化控制器的擺體角度與運動位移的輸出圖

4 結論與展望

兩輪自平衡機器人的核心是必須能夠在兩個車輪上保持平衡，否則機器人的所有其他功能都會崩潰。根據直流電機線性模型、車輪模型和擺體模型推導出兩輪自平衡機器人的運動控制數學模型并利用系統能控和能觀性判斷條件確定了系統具有能控和能觀性。但是由于某些特征值位于S平面的右半部分，因此系統是不穩定的。為了得到一套完整的穩定系統，結合狀態估計和反饋控制設計狀態估計反饋控制器并引入LQR控制方法優化控制器性能。系統仿真實驗結果表明，所設計的控制器對于移動機器人的平衡移動具有良好的控制效果。

系統具有完整的數學模型，因此，系統可以用作測試控制器的基準，對各種控制器的性能進行比較研究，例如，以設計非線性控制器并可以測試其性能，還可以定量地研究線性控制系統在實際非線性系統上的穩定性分析。

進一步的工作將通過添加視覺系統來提高機器人的自主水平，從而使機器人能夠避開障礙物。此外，通過改進機器人的組件，希望獲得更高的速度。另一個可能的擴展是將Arduino系統與其他傳感器相結合，例如超聲波和紅外感應、GPS、數字羅盤和相機，以解決其他高級應用程序，例如避障和周邊追隨。