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

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

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

1 引言

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

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

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

針對上述問題，本文提出了一種模糊PI控制算法對機器人的航向進行修正。該模糊控制器是以姿態(tài)角度傳感器反饋的航向角偏差和航向角偏差的增量作為輸入量，以模糊PI控制算法的參數(shù)Kp、Ki作為輸出量；模糊控制器對輸入、輸出量進行模糊化處理之后，通過模糊規(guī)則表對輸入的模糊量進行模糊推理來輸出模糊控制量，然后通過去模糊化把輸出的模糊模糊控制量轉(zhuǎn)變?yōu)榍逦等ジ淖働WM波的占空比，進而控制指定電機轉(zhuǎn)速，最終通過差分轉(zhuǎn)向技術去修正機器人的行駛航向，從而在不需要任何引導線和特殊標志的情況下，保證了機器人行駛航向精度。最后，通過實驗驗證了模糊PI控制器的有效性。

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

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

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

圖1 麥克納姆輪實物圖

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

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

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

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

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

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

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

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

(1)

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

(2)

由式(1)和(2)可得

(3)

求得:

(4)

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

(5)

其中J為系統(tǒng)逆運動學的雅克比矩陣。

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

3 控制系統(tǒng)總體結(jié)構(gòu)

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

圖4 控制系統(tǒng)結(jié)構(gòu)圖

圖5 機器人系統(tǒng)實物圖

4 模糊PI控制算法描述

4.1 機器人的調(diào)速控制系統(tǒng)

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

圖6 機器人的調(diào)速控制系統(tǒng)框圖

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

Δvalue=Kp×Δey+Ki×ey

(6)

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

4.2 模糊控制器的設計

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

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

模糊化是指把輸入及輸出量的實際值映射到相應的模糊論域中，使其值可以用“大”“小”等語句表示。本文采用了二維模糊控制，模糊控制器中的輸入量是移動機器人航向角的偏差ey及其偏差增量Δey，其輸出量是模糊PI控制器中的參數(shù)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}。

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

(7)

2)模糊推理

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

表1 模糊控制的PI參數(shù)整定表

根據(jù)Kp、Ki參數(shù)對控制系統(tǒng)性能的影響，來確定在不同的E和ΔE情況下確定調(diào)節(jié)指定電機轉(zhuǎn)速的Kp和Ki的值，來調(diào)節(jié)移動機器人的行駛航向。

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

圖7 小車前進左偏的調(diào)整示意圖

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

圖8 小車橫移左偏的調(diào)整示意圖

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

表2 Kp、Ki的模糊控制規(guī)則表

3)去模糊化

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

(8)

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

5 實驗與結(jié)果

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

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

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

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

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

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

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

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

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

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

表3 性能指標對比

6 結(jié)論

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

1)實現(xiàn)了全向移動機器人關于運動航向的模糊PI控制，解決了全方位移動機器人在運動時由于麥克納姆輪滑動而產(chǎn)生航向偏移的問題，提高了機器人運輸?shù)姆€(wěn)定性。

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

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