繆文南， 周 政， 旋極一， 陳東水， 曹鬧昌

(華南理工大學廣州學院 電子信息工程學院，廣東 廣州 510800)

0 引 言

單球自平衡移動機器人模型類似全方位倒立擺模型，騎在球體上可實現(xiàn)自平衡和可持續(xù)穩(wěn)定的運動控制，具有非線性、欠驅(qū)動、高階次、多變量等特點[1]。系統(tǒng)集先進控制理論、傳感器檢測、機電設計與控制等多學科于一體復雜的控制系統(tǒng)，不僅僅對提高機器人在復雜的環(huán)境下適應能力具有現(xiàn)實意義，還可以促進控制理論的發(fā)展[2]。在特定的環(huán)境下，利用機器人與地面單點接觸、無半徑和全向移動的靈活特點，可輔助或代替人在復雜地形條件下完成既定任務[3,4]。

單球自平衡移動機器人主要包含球輪、全向輪、機身三大部分，其中機身主要包含電氣控制系統(tǒng)、整體機械框架、載重組件等，涉及機械結構建模、姿態(tài)控制、運動軌跡導航與跟蹤等技術，引起了國內(nèi)外眾多學者的研究。據(jù)文獻記載，國外最早研究始于1980年日本Ozaka等人提出獨輪自平衡機器人；美國卡內(nèi)基梅隆大學Kantor GA等學者2005年推出了單球驅(qū)動機器人Ballbot。隨后瑞士蘇黎世聯(lián)邦理工學院在此基礎上重新設計了一種結構簡單的單球自平衡機器人[5，6]。國內(nèi)于2012年開始對球平衡機器人進行建模和研究，文獻[7]針對球平衡機器人運用拉格朗日方程法建立動力學模型，并進行線性二次型最優(yōu)控制方法進行控制機器人姿態(tài)。文獻[8]基于二維倒立擺模型建立動力學模型，提出遺傳算法的優(yōu)化控制機器人的原點自平衡控制。文獻[9]單球平衡器采用串級比例—積分—微分(proportional-integral-differential,PID)控制設計方法。綜合可見，目前對于單 球自平衡機器人的控制方法研究國內(nèi)還相對較少，控制方法主要以PI,PD,PID,LQR為主，單一的控制效果不理想，需要通過經(jīng)驗算法不斷修改相應的參數(shù)才能獲取較快的響應速度、調(diào)節(jié)時間等參數(shù)，已取得的成果更多也還在實驗探索階段。因此針對一個復雜的非線性系統(tǒng)，相關的理論和方法需要進一步探索和完善。

基于上述研究存在的問題，針對機器人平衡控制過程中穩(wěn)定性的要求，單球自平衡機器人建立運動學和動力學基礎上，基于角度傳感器和編碼器建立正運動學和逆運動學方程，設計了一種模糊PID算法的線性二次最優(yōu)控制的控制器，引入外部干擾，尋求提高系統(tǒng)的姿態(tài)平衡和移動過程中的魯棒性。通過實驗仿真驗證，檢驗單球機器人控制系統(tǒng)對機器人的姿態(tài)控制和運動控制的有效性和運動效果。

1 單球平衡機器人空間坐標系建模

單球平衡機器人起源于倒立擺模型，是一個非線性、強耦合、欠驅(qū)動、多變量和平衡不穩(wěn)定的復雜系統(tǒng)，為了簡化機器人控制模型，使用兩個一級倒立擺和IASMP模型等效球平衡機器人三個自由度[10]。以驅(qū)動球為中心建立直角坐標系O-XYZ，對機器人進行運動學和動力學模型分析，這樣有助于降低運算的復雜度，等效模型如圖1所示。其中θx,φx分別為YOZ上主體的旋轉角度和虛擬驅(qū)動輪的旋轉角度。

圖1 單球移動機器人結構圖

圖2 球與全向輪的位置關系

由圖2可得，三維直角空間坐標系O-XYZ的XOZ和YOZ平面模型等同于一級倒立擺模型，模型運動分析相同，只是一個左右運動，一個前后運動,而XOY平面模型類似太陽與地球相對位置的關系。因此，下面通過正運動學分析和逆運動學XOZ平面和XOY平面模型[10]。

(1)

(2)

圓球的運動方向可分為前進、后退、向左、向右、旋轉五個方向，根據(jù)運動學模型構建了三維立體坐標系O-XYZ，假設圓球按照X軸方向運動時，即為前后方向，圓球的角速度矢量為ω=(ωx,0,0)，則全向輪三個方向的驅(qū)動速度為

(3)

圓球按照Y軸運動方向時，即運動方向為左右方向,圓球的角速度矢量為ω=(0,ωx,0)；則全向輪三個方向的驅(qū)動速度為

(4)

圓球按照Z軸方向時，即為旋轉方向,,圓球的角速度矢量為ω=(0,0,ωx)，則全向輪三個方向的驅(qū)動速度為

|vsA|=|vsB|=|vsC|=-rbcosβ·ωz

(5)

設置球與地面的接觸點位置向量為Ri=[0,0,-rb]，則球與地面接觸點的速度矢量為

V=ω·Ri=-rbωBj+rbωAj+0k=vxi+vyj+vzk

(6)

綜合式(3)～(式)5，由此通過正運動學分析可推出三個方向的速度合成的三個全向輪的實際速度

(7)

同理，根據(jù)文獻[12]逆矩陣運動學分析可推導出球心方向速度Vi(i=X,Y)與全向輪線速度vi(i=A,B,C)之間的關系，則球運動方向速度為

(8)

(9)

圓球與全向輪的關系好比太陽與地球之間的關系，根據(jù)上面建立的XOZ和YOZ平面模型可建立拉格朗日方程，選取合適的廣義坐標研究系統(tǒng)的動能和勢能變化的關系，將平衡點線性化后得出X方向的動力學方程為

(10)

將表1的數(shù)據(jù)代入上述方程可獲取機器人姿態(tài)平衡和運動軌跡跟蹤的狀態(tài)。

表1 單球平衡機器人參數(shù)

2 單球平衡機器人控制系統(tǒng)研究

2.1 控制器設計思路

根據(jù)上面建立球移動平衡系統(tǒng)的動力學和運動學方程，設計了如圖3球移動平衡系統(tǒng)的控制系統(tǒng)結構圖，從圖中得知，球平衡系統(tǒng)包括控制、執(zhí)行、反饋三個單元。控制單元給定初始指令后，依據(jù)正運動學分析獲取的電機編碼器XY軸位移和速度，建立XY軸兩個平面的模糊PID控制器，控制機器人的X,Y軸方向運動；依據(jù)角度傳感器獲取XY軸的角度和角速度同樣建立兩個平面模糊PID控制器，控制機器人姿態(tài)控制或移動過程中X軸和Y軸的角度；通過模糊PID控制器和逆運動學分析將機器人運動方向的速度矢量轉換為全向輪的速度矢量，驅(qū)動電機進行控制球的直立平衡和移動。

圖3 機器人控制系統(tǒng)設計框圖

2.2 控制器設計

模糊PID控制方法是對PID算法的改進，以誤差 和誤差變化率 作為輸入，PID控制器的三個參數(shù)P，I，D的修正ΔKP0，ΔKI0，ΔKD0作為輸出。運用模糊規(guī)則進行模糊推理，查詢模糊子集{NB，NM，NS，ZO，PS，PM，PB}獲得PID控制參數(shù)KP,KI,KD的值并去模糊化，在不同時刻不斷檢測和調(diào)整誤差e和誤差變化率ec，滿足PID參數(shù)自整定的要求[13]，輸出模糊輸出量。如圖4所示。

圖4 模糊PID控制器

針對模糊PID控制器的整定參數(shù)較多，過程較為復雜，系統(tǒng)模糊PID控制器設計線性二次最優(yōu)控制方法，不斷提高系統(tǒng)的最優(yōu)控制和魯棒性。于是本文將上述表1參數(shù)代入二次線性方程[7]，獲得單球平衡控制系統(tǒng)XOZ，YOZ平面控制狀態(tài)空間模型，如式(11)所示

(11)

系統(tǒng)性能指標

(12)

當機器人收到外干擾時，采用狀態(tài)反饋形式優(yōu)化狀態(tài)控制器，通過試湊法調(diào)節(jié)u參數(shù)，逐漸讓系統(tǒng)達到原點保持平衡，提高控制器的性能。其中u(t)需要滿足u(t)=-Kx(t)，其中K=R-1BTP。

3 仿真測試與分析

單球平衡機器人的姿態(tài)控制包含直立平衡控制和軌跡跟蹤控制。其中球平衡機器人的直立平衡問題是最基礎最重要的問題，控制機器人的俯仰角和偏航角，通過控制器控制使得俯仰角和偏航角趨向0°。本文根據(jù)平衡控制思路及框圖設計，設置XOZ的初始狀態(tài)為X0=[x,θx,y,θy]=[0 0.4 0 0]，即X軸方向的初始傾角為0.4 rad，模糊PID自調(diào)節(jié)控制參數(shù)分別為KP0=50,KI0=85,KD0=4.5，PID控制參數(shù)初值與模糊PID控制參數(shù)初值一樣。利用仿真軟件調(diào)節(jié)獲取機器人XOZ平面的傾斜角和角速度曲線，實現(xiàn)機器人的平衡控制，如圖5所示。

圖5 機器人的姿態(tài)控制響應曲線

剛開始，控制器啟動加速，控制全向輪旋轉，機器人姿態(tài)在自動控制過程中逐漸平衡，經(jīng)過3.3 s，傾斜角及角速度變小都趨向0，表2數(shù)據(jù)分別為PID控制和模糊PID控制角度θx和角速度x的調(diào)節(jié)時間ts和超調(diào)量mp，從數(shù)據(jù)發(fā)現(xiàn)，模糊PID控制算法效果更佳。

表2 兩種算法的控制參數(shù)表

模糊PID調(diào)節(jié)器控制機器人需要經(jīng)過2.3 s達到平衡狀態(tài)，模糊PID控制器相對于傳統(tǒng)的PID控制，具有自動調(diào)節(jié)、響應快、穩(wěn)定型強等優(yōu)點，超調(diào)量也比較小。

為驗證球平衡機器人姿態(tài)的穩(wěn)定性和魯棒性，在10 s時給一個幅值為0.1的脈沖干擾，從圖6響應狀態(tài)過程，模糊PID控制器對脈沖干擾的響應效果更好，自動調(diào)節(jié)能力更強。

圖6 機器人在干擾下的姿態(tài)控制響應曲線

機器人機身平衡后，設定機器人初始化狀態(tài)參數(shù)為[0 0.1 0 0.1]，原始坐標為[0.5 0]。首先向X軸坐標行駛1.5 m，再向Y軸1.5m。

最后從坐標點[2,1.5]以直線方式向初始點[0.5,0]行駛。在仿真測試環(huán)境下，機器人在XOY平面圖的軌跡跟蹤如圖7所示。理論行駛軌跡為細線，PID行駛軌跡為粗線，模糊PID行駛軌跡則為虛線，模糊PID控制器和PID控制器的行駛軌跡跟蹤最大偏差分別為0.092 m和0.16 m。因此，模糊PID控制算法相比PID控制算法，機器人在啟動及移動過程中，機器人的俯仰角和和翻滾角調(diào)節(jié)時間更短，更快達到穩(wěn)定狀態(tài)，X軸和Y軸的軌跡跟蹤偏差更小。綜上所述，模糊PID控制算法的可靠性強，魯棒性更佳，更適用于機器人的實時的平衡控制和運動控制。

圖7 機器人三角形軌跡跟蹤仿真圖

4 結 論

針對單球平衡機器人系統(tǒng)的非線性、高階次、強耦合等特點，本文通過研究單球平衡機器人空間坐標系的運動學和動力學模型，設計了一種利用模糊PID算法的線性二次型最優(yōu)控制方法控制器，控制單球平衡機器人的平衡姿態(tài)和全向移動。仿真測試表明，該改進的模糊PID控制器相比傳統(tǒng)PID控制器可以更好實現(xiàn)機器人的動態(tài)平衡和全方位移動控制，具有調(diào)節(jié)過程平穩(wěn)、響應速度較快等優(yōu)點，同時該控制器對外界干擾具有較好的穩(wěn)定型和魯棒性。該設計為球平衡機器人提供了新的思路。