全向移動機器人模糊PI控制研究

張 鵬，牛子杰，王龍寧，劉坤澎

(西北農林科技大學機械與電子工程學院，陜西 西安 712100)

1 引言

基于麥克納姆輪的全向移動機器人能實現前進、橫移、360°零半徑旋轉等基本運動和復合運動[1]，能實現在平面內以任意方位移動和任意半徑旋轉，故在運輸平臺、加工平臺、現代制造行業等眾多領域開始發揮著越來越重要的作用[2，3]。

全向移動機器人通常是由四個麥克納姆輪按照一定布局組合在一起，依靠這四個輪子各自不同的轉速實現全向移動[4]。麥克納姆輪在運動時只有一個小輥子單點接地，輪子的運動極易受地面摩擦力的影響[5-6]。若其中一個輪子出現與地面脫離接觸或打滑等情況，則移動機器人的運動航向就會發生偏斜，導致全向移動機器人的實際運行航向和位置偏離機器人設定航向和位置[7]。

為了克服全向移動機器人的這一問題，很多學者都研究了通過檢測麥克納姆輪滑動量來糾正機器人位置的方法。Patrick等人闡明了基于麥克納姆輪機器人的運動學方程，并提出一種檢測滑動的方法，然而該檢測方法需要依賴于能夠檢測絕對位置的附加設備[8]；Luo等人對全向移動機器人所受到的不平衡的法向力所引起的打滑誤差進行了建模和分析，他們還提出了一種基于麥克納姆輪機器人的在線自適應滑動模型以減少滑動誤差[9]；王明明等人將自適應魯棒控制應用于麥克納姆輪驅動的移動機器人軌跡跟蹤以獲得良好的動態跟蹤性能以及魯棒性能，提出了一種比例-積分-微分(PID)形式的滑模面，滿足了系統的魯棒性要求，并設計了一種能夠快速收斂的趨近律，減少了整定參數所消耗的時間并能有效抵抗外部擾動[10]。在智能控制領域，許多研究人員表示模糊控制理論可以被用來去克服麥克納姆輪滑動的這一難題；模糊控制的基本思想最早是由Zadeh和Mamdani[11，12]提出，后來是Christopher M.Kumile等人在基于麥克納姆輪的自動導引小車系統中實現了智能分布式模糊控制，根據有無打滑的情況進行不同的處理去改進控制系統的可靠性和降低控制器合成的復雜性[13]。Tsai等人提出了一種基于模糊小波神經網絡的非奇異終端滑模控制，并用此方法來提高基于麥克納姆輪的全向移動機器人的軌跡跟蹤的準確性和行駛穩定性[14]。Fahmizal等[15]利用人工墻壁作為參考點，通過超聲波傳感器反饋距離信息給模糊控制器來控制差速轉向機器人沿著設定的航向行走，后來他們還通過姿態角度傳感器和模糊控制器相結合進行了全向移動機器人定向運動的控制與仿真[16]。

針對上述問題，本文提出了一種模糊PI控制算法對機器人的航向進行修正。該模糊控制器是以姿態角度傳感器反饋的航向角偏差和航向角偏差的增量作為輸入量，以模糊PI控制算法的參數Kp、Ki作為輸出量；模糊控制器對輸入、輸出量進行模糊化處理之后，通過模糊規則表對輸入的模糊量進行模糊推理來輸出模糊控制量，然后通過去模糊化把輸出的模糊模糊控制量轉變為清晰值去改變PWM波的占空比，進而控制指定電機轉速，最終通過差分轉向技術去修正機器人的行駛航向，從而在不需要任何引導線和特殊標志的情況下，保證了機器人行駛航向精度。最后，通過實驗驗證了模糊PI控制器的有效性。

2 全向移動機器人的運動學分析

2.1 麥克納姆輪的全向移動機理

本文所使用的麥克納姆輪實物如圖1，本文采用的是設計成45°夾角的麥克納姆輪。小輥子不僅可以繞車輪軸線進行公轉和繞輥子的軸線進行自轉，還能繞輥子與運動地面的接觸點進行轉動，這是麥克納姆輪具有3個自由度的運動特點[17-18]。

圖1 麥克納姆輪實物圖

麥克納姆輪的突出應用是通過使用該輪讓機器人的移動機構變成全向移動機構，實現橫向、縱向、零半徑旋轉三個方向的運動[19]。本文全向移動機器人的驅動方式是四輪單獨驅動，每個麥克納姆輪都配備一個直流電機來進行控制。機器人在運動時，麥克納姆輪的小輥子受到地面所給的兩個方向作用力：一個是輥子軸向方向的滑動摩擦力，另一個是輥子圓周方向的滾動摩擦力；由于輥子所受的滾動摩擦力可以近似為零，故只需看輥子所受的軸向摩擦力對機器人運動的影響。因此，麥克納姆輪與地面的接觸力不在沿著輪子的圓周方向，而是與輪子的圓周方向成45°夾角。通過對不同的麥克納姆輪所受軸向力的合成，就能判斷機器人的運動方向。本文設計中使用了4個麥克納姆輪，兩兩鏡面對稱安裝，圖2是本文設計中麥克納姆輪的布局方式和各輪受力方向分析圖，圖中車輪斜線為與地面接觸的小輥子，結合車輪的運動方向和受力方向展示了四輪移動機器人的前進、橫移、側移、旋轉運動產生的機理。以前進為例，如圖2(a)，此時四個輪子的角速度相同，每個輪子的小輥子軸線方向都有獨立的軸向摩擦力，通過軸向摩擦力的合成，小車所受的橫向分力被相互抵消，只保留了前進方向的分力，從而讓移動機器人產生向前的運動。

圖2 機器人不同運動模式下的受力

Fa：小輥子受到的軸向摩擦力

Fr：小輥子受到圓周方向的滾動摩擦力

2.2 移動機器人的運動學分析

如圖3是移動機器人的運動學分析模型圖，以移動機器人的幾何中心O為坐標原點，建立全局坐標系XOY，以車輪的轉動中心Oi為坐標原點，分別建立四個局部坐標系XiOiYi，圖中r為車輪轉動的有效半徑，a為車體幾何中心O到各輪轉動中心Oi的水平距離，b為車體幾何中心O到各輪轉動中心Oi的垂直距離，輥子的軸線與車輪軸線的夾角β為45°。各輪角速度分別為ωA、ωB、ωC、ωD，以車輪逆時針轉動為正，順時針轉動為負，各輪小輥子的速度分別為vg1、vg2、vg3和vg4，小車的橫向速度、縱向速度和自轉角速度分別是Vx、Vy和ω，機器人逆時針方向轉動。

圖3 移動機器人的運動學分析模型

以輪A為例，車輪轉動中心OA在全局坐標系XOY中的移動速度為

(1)

OA在局部坐標系X1OAY1的移動速度為

(2)

由式(1)和(2)可得

(3)

求得:

(4)

同理，對其余輪子進行運動學分析，可得：

(5)

其中J為系統逆運動學的雅克比矩陣。

由逆運動學原理可知，當系統的逆運動學雅克比矩陣是奇異矩陣時，系統的運動自由度就會減小；當運動系統的逆運動學雅克比矩陣是非奇異矩陣時，就具有全向移動的能力。對照式(5)中的矩陣J，可知rank(J)=3，故本文設計的系統具備全向移動的能力。另外式(5)反映了各輪角速度與系統中心速度的映射關系，只要合理的控制四個輪子的角速度就能實現全向移動機器人的各種運動形式。

3 控制系統總體結構

控制系統結構圖如圖4，主要由微控制器、電機驅動器、編碼器、姿態角度傳感器、直流電機、上位機構成。微控制器STM32F103是整個控制系統的核心，進行各種數據計算與反饋信息的快速處理。控制系統通過BTN7971B電機驅動器來驅動直流電機，通過霍爾編碼器反饋電機速度給微控制器進行速度調節；姿態角度傳感器選擇了JY901模塊，該模塊集成高精度的陀螺儀、加速度計、地磁場傳感器，芯片內部采用了先進的動力學解算與卡爾曼動態濾波算法，能夠在動態環境下準確輸出模塊的當前姿態，姿態測量精度靜態0.05度，動態0.1度，穩定性極高，能夠快速求解出模塊當前的實時運動姿態，最終選擇以I2C通信方式實現JY901模塊與微處理器進行通信來實現姿態角的傳輸；機器人姿態角包括的俯仰角、橫滾角和航向角，本文主要是利用反饋的航向角進行機器人實時的航向修正，保證機器人指定航向運動的準確性。另外本文通過使用DL-20無線串口模塊把機器人航向偏差和電機角速度上傳到上位機中進行數據的記錄。根據機器人控制系統結構圖搭建的機器人實物圖如圖5。

圖4 控制系統結構圖

圖5 機器人系統實物圖

4 模糊PI控制算法描述

4.1 機器人的調速控制系統

機器人的調速控制系統框圖如圖6，圖中value和Δvalue的值是賦給STM32F103高級定時器的CCRx寄存器，通過對CCRx寄存器的不同賦值便能產生不同的PWM輸出波控制直流電機的轉向與轉速。在沒有航行角的反饋調節時，對電機速度的調節采用了傳統速度PI控制算法輸出的value值對電機的速度進行精準控制；當設定機器人的移動速度之后，通過式(5)計算出每個電機的目標轉速，通過編碼器對電機的實際轉速進行反饋，求出電機目標速度與實際轉速的偏差e，速度PI控制器通過偏差e計算出value變量的數值，進而產生PWM波來實現對電機速度的精準控制。

圖6 機器人的調速控制系統框圖

通過反饋的航向角對機器人進行航向修正的算法采用了模糊PI控制算法。如圖6，模糊PI控制器是由傳統航向角PI控制器和模糊控制器組成，在控制上不使用固定參數的PI調節算法，而是針對不同的情況，由模糊控制器實時對PI參數進行調節。通過姿態角度傳感器JY901對機器人的航向進行實時反饋，當機器人的實際航向偏離設定航向時，控制系統就會檢測到機器人航向角的偏差ey(ey=Yaw-PreYaw)和航向角偏差的增量Δey=(ey-Last_ey)，并把ey和Δey送給模糊控制器，來實時對模糊PI控制的兩個參數Kp、Ki進行動態調節。模糊PI控制的基本表達為

Δvalue=Kp×Δey+Ki×ey

(6)

航向角PI控制器通過式(6)產生的Δvalue調節機器人指定電機的轉速，進而修正機器人的運動航向，當修正到機器人的設定航向時，Δvalue就會自動變成0，此時根據傳統速度PI控制算法又讓電機回到了原始目標速度。

4.2 模糊控制器的設計

模糊PI控制算法的關鍵是對模糊控制器的設計，本文提出的模糊控制器的設計過程可以分3個步驟：模糊化、模糊推理、去模糊化。

1)輸入域輸出量的模糊化

模糊化是指把輸入及輸出量的實際值映射到相應的模糊論域中，使其值可以用“大”“小”等語句表示。本文采用了二維模糊控制，模糊控制器中的輸入量是移動機器人航向角的偏差ey及其偏差增量Δey，其輸出量是模糊PI控制器中的參數Kp和Ki。偏差ey的模糊語言變量是Ey，其論域是{-5，-4，-3，-2，-1，0，1，2，3，4，5}；模糊變量Ey的語言值是{負大，負中2，負中1，負小2，負小1，零，正小1，正小2，正中1，正中2，正大}，表示為：{NB，NM2，NM1，NS2，NS1，ZO，PS1，PS2，PM1，PM2，PB}。偏差增量Δey的模糊語言變量是ΔEy，其論域是{-3，-2，-1，0，1，2，3}，模糊變量ΔEy語言值是{負大，負中，負小，零，正小，正中，正大}，表示為：{NB，NM，NS，ZO，PS，PM，PB}。模糊語言變量Kp和Ki的模糊語言變量是Kpp和Kii，Kpp和Kii論域是{-4，-3，-2，-1，0，1，2，3，4}，模糊變量Kpp和Kii的語言值是{負大，負中2，負中1，負小，零，正小，正中1，正中2，正大}，表示為{NB，NM2，NM1，NS，ZO，PS，PM1，PM2，PB}。

把輸入量和輸出量與模糊變量語言值相匹配的過程需要使用隸屬度函數，它表示真實值與語言值相符合的程度，即一個具體的數值在多大程度上可以用該語言迸行描述。本文設計中隸屬度函數采用了三角形隸屬函數，其數學表達式如式(7)所示，式中的a、b、c為模糊變量論域區間常數。

(7)

2)模糊推理

模糊控制器的核心任務就是確定模糊規則并對模糊結果進行推理，模糊規則是在前人長期積累的基礎上，在通過實驗對參數值的變化范圍進行確定。模糊推理的形式是：if(E is Ai)and(ΔE is Bi)，then(Kp is Ci1)and(Ki is Ci2)。模糊控制中PI參數整定表見表1。

表1 模糊控制的PI參數整定表

根據Kp、Ki參數對控制系統性能的影響，來確定在不同的E和ΔE情況下確定調節指定電機轉速的Kp和Ki的值，來調節移動機器人的行駛航向。

如圖7當機器人在縱向運動時左偏Δθ之后，通過模糊PI控制去自動快速增加左側A、B電機的轉速來讓機器人在縱向行駛中差速右轉進行修正行駛航向；當機器人即將回到設定行駛航線時，A、B電機自動減速到原始轉速來減少航向修正過度的現象，保證機器人縱向行駛的航向精度；同理當機器人在縱向運動右偏時，是右側C、D電機加速來修正機器人的方向。

圖7 小車前進左偏的調整示意圖

如圖8是機器人在向右橫向運動時向左偏移Δθ角度之后，微控制器通過模糊控制算法增加B、C電機在其原始轉向上的轉速，從而讓機器人在橫向行駛的過程中產生順時針的轉動來修正機器人橫向運動的方向，保證機器人橫向行駛的航向精度。同理當機器人在橫向運動右偏時，是通過增加A、D電機在其原始轉向上的轉速，產生逆時針的轉動修正機器人的航向。

圖8 小車橫移左偏的調整示意圖

根據上文對機器人縱向行駛過程中發生左右偏斜時機器人航向修正機理的分析，以機器人縱向運動為例建立模糊推理規則表。在軟件編程中實現了Kp、Ki對電機轉速的統一化影響，即Kp、Ki取正值時電機會加速，Kp、Ki取負值時電機會加速，并由式(6)可知Kp、Ki的值越大，對應電機加速越快。最終結合前人經驗和實驗的修正效果得到了輸出量Kp、Ki的模糊控制規則見表2。

表2 Kp、Ki的模糊控制規則表

3)去模糊化

通過模糊規則進行推理后得到的是模糊語言變量，不能直接用于控制，因此要通過去模糊化把模糊語言變量轉化為清晰量，本文選擇了重心法去模糊化，其數學表達式如式(8)。

(8)

式中，<>代表四舍五入取整操作，z0為清晰值，zi為模糊變量，μ(zi)為隸屬度值。

5 實驗與結果

本文以機器人縱向運動和橫向運動為例進行實驗分析。當機器人在不利用反饋的航向角進行航向修正的前提下分別縱向、橫向行駛3.5m時，機器人的實際航向與設定航向之間的航向角偏差(Error of yaw，EOY)如圖9。從實驗結果可以看出由于麥克納姆輪的滑移現象和機器人的動態不平衡性，機器人在運動3.5m的距離中縱向運動會產生2°左右的航向偏差，橫向運動會產生10°左右的航向偏差。

圖9 機器人無反饋運動時的航向角偏差

針對機器人航向偏移的問題，本文設計了模糊PI控制算法修正機器人的航向。圖10是機器人分別在橫向、縱向移動時利用模糊PI控制算法去進行反饋控制時的航向角偏差變化圖。多次實驗結果表明機器人在縱向、橫向直行運動的過程中航向角偏差都在[-1，1]之間變化，可以看出在使用了模糊PI控制算法進行航向角反饋修正之后，機器人的縱向、橫向的直行效果有了明顯改善，航向精度得到很大地提高。

圖10 機器人模糊PI控制的航向角偏差

當航向角的偏差有明顯變化時，利用航向角進行反饋修正航向的傳統PI控制算法和模糊PI控制算法在調節性能上有明顯的差異。在機器人運動的過程中人工給予一個很大的擾動，讓機器人的實際航向角遠離設定航向角來產生一個大幅度的航向角偏差。如圖11是機器人縱向運動時在大擾動下的傳統PI控制算法和模糊PI控制算法的航向角反饋修正對比圖。如圖12是機器人橫向運動時在大擾動下的傳統PI控制算法和模糊PI控制算法的航向角反饋修正對比圖。

圖11 縱向行駛時在大擾動下的傳統PI控制與模糊PI控制的反饋修正對比圖

圖11(a)是機器人縱向移動時在大擾動下的模糊PI控制算法修正的航向角偏差變化圖，圖11(b)是縱向移動利用模糊控制調整機器人保持設定航向時的對應電機調速圖，由于在縱向運動修正航向的過程中電機A、B的轉速相同，電機C、D的轉速相同，故只表示了A、D電機的轉速。圖12(a)是機器人橫向移動時在大擾動下的模糊PI控制算法修正的航向角偏差變化圖，圖12(b)是縱向移動利用模糊控制調整機器人保持設定航向時的對應電機調速圖，由于在橫向運動修正航向的過程中電機A、D的轉速大小相同，電機B、C的轉速大小相同，故只表示了A、B電機的轉速。同理、圖11(c)和圖11(d)分別是縱向運動在大擾動時傳統PI控制下的航向角偏差變化圖和電機調速圖，圖12(c)和圖12(d)分別是橫向運動在大擾動時傳統PI控制下的航向角偏差變化圖和電機調速圖。

圖12 橫向行駛時在大擾動下的傳統PI控制與模糊PI控制的反饋修正對比圖

圖11(a)中當航向角偏差是正值時，表示機器人是在縱向行駛時向左偏斜，從圖11(b)可以看出當機器人向左偏斜之后，A、B電機的轉速快速上升調整機器人的航向，恢復到設定航向。圖12(a)中當航向角偏差是正值時，表示機器人是在橫向行駛時向左偏斜，從圖12(b)可以看出當機器人向左偏斜之后，A、D電機的轉速快速上升調整機器人的航向恢復到設定航向。在縱向運動中圖11(c)相比圖11(a)調整時間長，超調量大，同樣圖11(d)相比圖11(b)電機轉速超調量大，調整時間長。在橫向運動中圖12(c)相比圖12(a)調整時間長，超調量大，同樣圖12(d)相比圖12(b)電機轉速超調量大，調整時間長。

實驗結果對比表明，模糊控制有電機加速快、減速到目標速度快、航向角修正時的超調量小，調整時間快。當最大航向角偏差在35～40°之間時，利用模糊PI控制算法和傳統PI控制算法進行反饋修正時的性能指標對比見表3。由表3可知模糊PI控制系統的動態性能、穩定性比傳統PI控制系統好，既能減少振蕩，又能較快調節。

表3 性能指標對比

6 結論

針對基于麥克納姆輪的全向移動機器人在運動時由于麥克納姆輪的滑動而產生航向偏移的問題，本文提出了利用姿態角度傳感器反饋的航向角的對機器人進行運動航向修正的模糊PI控制算法。

1)實現了全向移動機器人關于運動航向的模糊PI控制，解決了全方位移動機器人在運動時由于麥克納姆輪滑動而產生航向偏移的問題，提高了機器人運輸的穩定性。

2)搭建了實驗環境，通過實驗比較了有無模糊PI控制算法對機器人航向角偏差的影響情況，實驗數據表明，當機器人分別在縱向和橫向行駛3.5m的距離，機器人的航向角偏差分別由原始的0～2.5°和0～10.5°縮減到-0.6～0.6°和0～1°之間，極大的減小了運動航向偏差，保證了設定航向運動的精度。

3)通過實驗比較了傳統PI控制算法和模糊PI控制算法在機器人航向調節性能上的優劣性，實驗數據表明當機器人的航向角偏差在35～40°之間時，機器人在縱向運動時模糊PI控制算法的調節時間加快了1.3s，超調角度減小了7.7°；機器人在橫行運動時模糊PI控制算法的調節時間加快了1.2s，超調角度減小了4.3°。經實驗對比，模糊PI控制算法能根據機器人航向角偏差的當前狀態，利用模糊規則實時調整機器人的運動狀態，達到了響應速度快，控制更精準的效果。