基于模糊PD控制的四輪驅動全向移動機器人速度補償控制器研究

摘 要：由于四輪驅動全向移動機器人的四個輪子之間存在機械差異與耦合關系，即使單個電機的控制參數達到最優，整個機器人的控制效果也未必最佳。針對這一問題，設計一種速度補償控制器，基于模糊控制與PD控制理論，能夠在各種誤差e與誤差變化量Δe的情況下，對機器人四個輪子的速度進行有效補償。通過在Matlab-Simulink環境下的仿真實驗表明，在使用速度補償控制器后，機器人對線速度及角速度的跟隨性能明顯提高，改善了軌跡跟隨的精度。

關鍵詞：全向移動機器人；速度補償器；模糊-比例微分控制；運動控制

中圖分類號：TP242.6 文獻標識碼：A

Velocity Compensate Controller of Fourwheel Drive Omnidirectional Mobile Robots Based on FuzzyPD Control Method

KUANG Jianhui，YANG Yimin

(GuangDong University of Technology，GuangZhou 510090，China)

Abstract:Fourwheel Drive Omnidirectional Mobile Robots has mechanical difference in four wheels. Even each motor has the optimal parameter， the Robot may not be precisely controlled. This paper proposes an velocity compensate controller based on FuzzyPD control method， which can compensate velocity for each wheel real-timely at various error and error rate. The result of simulation experiment in Matlab-Simulink environmental illustrate that the Robot has a obvious improvement in following line speed and Angular velocity，as result，better performance in trajectory tracking.

Key words:Omnidirectional Mobilerobots; velocity compensate controller; FuzzyPD control; motion control

1 引 言

近年來，像全自主足球機器人這樣的全方位移動機器人由于控制簡單、移動靈活，已經被廣泛應用于人類的生產、生活實踐中，而且有著廣闊的應用前景，得到了越來越多的關注。全方位移動機器人采用全方位輪這種特殊的驅動機構，在二維平面上具有3個自由度，能夠沿任意方向同時做平動與轉動，其移動的快速性和機動性要優于自由度少于3個的非全方位移動機器人^［1］。

本文所研究的全自主足球機器人采用4輪式全方位輪結構，每個全方位輪由帶碼盤的直流無刷電機驅動。然而，由于機器人四個輪子之間存在耦合關系，即使對單個輪子的控制達到最優，對于整個機器人而言效果未必最佳。換而言之，控制系統不僅對單個電機要有滿意的控制效果，還必須能夠有效地減小機器人運動的整體誤差。針對這一問題，通常的解決方法是通過對機器人建立精確的運動學和動力學模型，從而得到控制量與狀態量的關系方程^［2，3］。這種方法雖能得到比較精確的控制方程，但計算復雜且需要知道機器人確切的機械參數與環境參數。本文通過在控制環路中添加基于模糊PD控制的速度補償器來解決這一問題。

2 四輪全向移動機器人運動學模型

首先給出機器人體坐標系即相對坐標系的定義。機器人體坐標系是一個以機器人的全向視覺系統的中心位置為原點，以機器人正前方為橫坐標方向的右手坐標系，如圖1所示。Vx為機器人體坐標系橫坐標方向線速度，Vy為機器人體坐標系縱坐標方向線速度，為機器人角速度，角速度正方向為機器人體坐標系逆時針方向。記(Vx Vy )為機器人的速度向量（以下簡稱速度），v1、v2、v3、v4分別為機器人1號、2號、3號、4號四個輪子的線速度。根據機器人四個輪子的布局，δ1、δ2分別為45°和60°，驅動輪中心到車體中心的距離R為22.5 cm，驅動輪半徑r為10 cm。

圖1 機器人體坐標系及速度分解示意圖

機器人世界坐標系定義為以場地中心為原點，對方球門方向為正方向的右手坐標系。根據圖1所示幾何關系，四個輪子的轉速分解如下：

v1v2v3v4=－cos δ1－sin δ1R－sin δ2cos δ2Rsin δ2cos δ2Rcos δ1－sin δ1RVxVy(2.1)

故四個輪子的期望轉速為：

ω1ω2ω3ω4=v1v2v3v4Krate(2.2)

ω1、ω2、ω3、ω4分別為四個輪子的轉速，Krate為輪子線速度與轉速的換算比。由v=ωr可知：

Krate=ωv=1r(2.3)

3 運動控制系統體系結構圖

運動控制系統體系結構如圖2所示。機器人的線速度由加速度計測得的線加速度經積分后獲得，角速度由陀螺儀測得。整個控制系統包含兩個閉環。外環將期望的速度同反饋回的速度做差得到機器人的速度誤差，速度補償控制器依據誤差及誤差變化率得到相應的速度補償量。然后將速度補償量與期望速度相加，即對期望的速度進行修正。之后再依據公式2.1分解出各輪的期望轉速。這實際上將速度補償量分配到了四個輪子，實現了對四個輪子的速度補償。而內環則是一個典型的直流電機調速系統，其反饋量是通過電動機光電編碼盤返回的正交脈沖信號得到的。對于內環無刷直流電機調速系統本文不多做論述。我們默許單個無刷直流電機調速系統已有足夠的精度，本文著重討論外環速度補償控制器的設計。

圖2 運動控制系統體系結構框圖

4 速度補償控制器設計

4.1 控制器結構設計

記期望的機器人的速度為(Vxs Vys s)，反饋回的機器人的速度為(Vx' Vy' ')，則速度誤差為(eVx eVy e)=(Vxs－Vx Vys－Vy s－'）。由于反饋回的線速度是由加速度計測得的線加速度經積分得到的，其精度并不是很高。因此外環速度補償控制器采用高精度的控制算法意義不大，而應在保證控制的快速性，穩定性與魯棒性的前提下盡可能提高精度。由于機器人在體坐標系的x和y方向有相似的運動特性，因此我們可以將速度誤差分為線速度誤差(eVx eVy)與角速度誤差e兩部分，分別通過一個線速度補償控制器和一個角速度補償控制器。

速度補償控制器的輸入量為速度誤差(eVx eVy e)，輸出量為速度補償量(uVx uVy u)。我們知道數字PID控制的常用表達式為：

u(k)=Kp{e(k)+TTi∑ki=0e(i)+TdT［e(k)－e(k－1)］}

=Kpe(k)+Ki∑ki=0e(i)+Kd［e(k)－e(k－1)］(4.1)

式中：T為采樣周期；k為采樣次數；Kp為比例作用系數，Ki=KpTTi為積分作用系數；Kd=KpTdT為微分作用系數。由公式4.1第二項可知，積分項依賴于以往所有的誤差。而前面提到，反饋回的線速度的精度并不是很高，若引入積分項反而可能累積出更大的誤差，同時也大大增加了計算量。因此本文采用PD控制的方法，其表達式如下：

u(k)=Kpe(k)+Kd［e(k)－e(k－1)］(4.2)

記線速度補償控制器的參數為KpV、KdV，角度補償控制器的參數為KP、Kd。KpV、KdV、KP、Kd均為正數，由4.2式可得：

uVx(k)uVy(k)=KpVeVx(k)eVy(k)+

KdVeVx(k)－eVx(k－1)eVy(k)－eVy(k－1)(4.3)

u(k)=Kpe(k)+Kd［e(k)－e(k－1)］(4.4)

由于兩個速度補償控制器結構相似，我們不妨考察對Vx的補償。記輪速分配的輸入量為VxVy，結合圖2、公式4.3、公式4.4可知：

VxVy=

Vxs+uVx(k)Vys+uVy(k)s+u(k)(4.5)

對于Vx，當k-1與k時刻均有期望速度與反饋速度相等，即Vxs=Vx'時，eVx=0，uVx(k)=0，Vx=Vxs。當Vxs＞Vx'，即eVx(k)＞0，說明實際速度低于期望速度，若更有eVx(k)－eVx(k－1)＞0，說明誤差在擴大，此時公式4.3右邊兩項均為正值，則得到較大補償量uVx(k)，使得Vx＞Vxs。補償量通過速度分配補償到各個輪子上后使得機器人增加x方向的速度從而減小誤差。此時則有eVx(k)－eVx(k－1)＜0，公式4.3右邊第二項變為負值，補償量uVx(k)變小，有利于機器人速度變化平滑，減小振動。其他情況的分析方法類似。

4.2 PD控制器參數整定

在機器人控制算法中，PID和PD是最簡單的控制算法。其控制算法不僅簡單，而且參數調整方便，魯棒性好，可靠性高。然而它是基于對象數學模型的方法，尤其適用于可建立精確數學模型的確定性控制系統。由于足球機器人是一個多變量，強耦合，非線性時變的復雜系統，當系統負載或參數發生變化時，采用PD控制難以保持設計時的性能和預期效果。然而，模糊控制不要求掌握被控對象的數學模型，能夠克服非線性因素的影響，對調節對象的參數變化具有較強魯棒性，并能夠減小超調量，提高抗干擾能力，縮短調節時間^［4，5］。基于以上原因，本文結合模糊控制與PD控制的優點，設計一種基于模糊控制的具有參數自整定功能的模糊PD控制器。

圖3 參數自整定模糊PD控制器

如圖3所示，我們通過參數自整定模糊規則對PD控制器的Kp，Kd參數進行整定。具體的，在系統運行中不斷檢測誤差e和誤差變化率Δe，根據模糊控制的原理對兩個參數進行在線修正以滿足不同工況（表現為e和Δe）下對（Kp，Kd）的不同要求，最終獲得良好的動態和靜態控制性能。總結以往控制系統的設計經驗：當|e|較大時Kp應較大而Kd應較小，這樣可使系統響應加快，令機器人可靠和快速的回到期望的軌跡上來；當|e|中等時，Kp應較小，使超調量較小，Kd取值要適當，因為它對系統的響應影響較大；當|e|較小時，Kp應較大，使系統具有較好的穩態性能，同時，Kd要適當，避免在平衡點附近出現振蕩^［6］。

根據以上分析，我們把|e|和|Δe|各分割成兩個模糊子集，表示為“大”（L）、“中”（M）、“小”（S），其隸屬函數分別如圖4的a、b所示，其形狀可以通過參數a1，a2，a3和b1，b2，b3來調整。

圖4 語言變量|e|和|Δe|的隸屬函數

根據|e|和|Δe|的不同組合和需要得到相應工況下參數自整定的模糊控制規則如下：

rule1:IF［＜|e|=L＞and＜|Δe|=L or Μ＞］

THEN＜Kp=Kp1，Kd=0＞

rule2:IF［＜|e|=L＞and＜|Δe|=S＞］

THEN＜Kp=Kp1，Kd=Kd1＞

rule3:IF［＜|e|=M＞and＜|Δe|=L or M＞］

THEN＜Kp=Kp2，Kd=Kd2＞

rule4:IF［＜|e|=M＞and＜|Δe|=S＞］

THEN＜Kp=Kp2，Kd=Kd3＞

rule5:IF［＜|e|=S＞and＜|Δe|

=L or M or S＞］

THEN＜Kp=Kp3，Kd=Kd3＞

|e|和|Δe|在各種組合下的Kp，Kd值通過湊試法得到。由于較大的|e|和|Δe|一般出現在機器人啟動加速時，較小的|e|和|Δe|一般發生在機器人直線勻速運動時，而中等的|e|和|Δe|則大多發生在機器人弧線行走時。因此，根據實際情況，對于較大的|e|和|Δe|組合，我們反復湊試參數Kp，Kd，直到機器人啟動速度達到滿意為止；對于較小的|e|和|Δe|組合，我們湊試一組Kp，Kd，使得機器人能走比較精確的直線；而對于中等的|e|和|Δe|組合，則湊試一組Kp，Kd，使機器人能做比較精確的圓周運動；對于其他的|e|和|Δe|組合則可根據實際情況對以上三種情況湊得的Kp，Kd參數進行組合。以上規則中Kp1，Kd1為機器人直線測試湊得的參數，Kp2，Kd2為機器人圓周測試湊得的參數，Kp3，Kd3為機器人啟動測試湊得的參數。

5 仿真實驗

本文在MatlabSimulink環境下對機器人的運動控制系統進行建模和仿真分析。為了便于分析，根據實際應用情況我們作如下的假設：①機器人是在一個平坦的表面上運動，以忽略地面的不規則情況；②輪子與地面之間點對點的滾動摩擦小到可以使車輪滾動；③機器人是鋼體，不考慮形變的情況。另外，由于仿真實驗中無法使用加速度計及陀螺儀來測量機器人的速度，我們可以根據公式2.1、2.2由四個輪子的轉速得到機器人的實際速度：

Vx'Vy''=

1400012(sin δ1+cos δ2)00012R(sin δ1+cos δ2)

－1cos δ1－1sin δ21sin δ21cos δ1－111－1cos δ2sin δ1sin δ1cos δ2ω1'ω2'ω3'ω4'1/Krate (5.1)

Matlab仿真實驗中無刷直流電機的性能參數為：額定功率60 W；額定轉矩85 mNm；額定轉速8 050 rpm；額定電壓24 V；額定電流3.44 A；轉子慣量33.3 gcm2；電樞電阻0.611 Ω；電樞電感0.119 mH；轉矩常數25.9 mNm/A；速度常數369 rpm/V；機械時間常數3.03 ms。電機減速器參數為：減速比1∶22；轉動慣量0.8 gcm2。機器人參數為：質量23 kg；半徑22.5 cm；輪半徑10 cm；單輪摩擦力矩1.86Nm。

為了測試機器人對線速度及角速度的跟隨性能，仿真實驗讓機器人在世界坐標系中走S型曲線。具體的給定機器人體坐標系的x方向線速度為1 m/s，y方向線速度為0 m/s，機器人體坐標系x軸的正方向指向世界坐標系y軸的正方向。機器人的朝向角以θ=π4sin (πt)的規律變化，故機器人的角速度為=π24cos (πt)。實驗通過啟用速度補償器和未啟用速度補償器兩種情況進行比較，得到四個輪子的轉速曲線、機器人的速度曲線及機器人的運行軌跡分別如圖5的a、b、c所示。

結果分析：當未啟用速度補償器時，在機器人運行軌跡的曲率較大處，四個輪子（尤其是1號和3號輪子）的轉速有較大的波動，從而使得機器人的線速度也有較大波動。由于仿真實驗并未對機器人引入位置反饋（事實上機器人的位置反饋是通過

(a)四個輪子的轉速曲線

(b)機器人的速度曲線

(c) 機器人的運行軌跡

……期望速度曲線及運行軌跡

┅未啟用速度補償器時的速度曲線及運行軌跡

啟用速度補償器時的速度曲線及運行軌跡

圖5 S型軌跡跟隨實驗結果圖示

全向視覺的自定位實現的），使得速度誤差累積到位置誤差上，造成機器人的運行軌跡與期望軌跡相比有較大的偏差；而在啟用速度補償器后，四個輪子的轉速的波動明顯減小，使得機器人對期望線速度和角速度的跟隨大為改善，機器人運行的軌跡也更加接近期望軌跡。

6 結束語

由于四輪驅動全向移動機器人的四個輪子之間存在耦合關系，即使單個電機的控制參數達到最優，整個機器人的控制效果也未必最佳。另外，由于場地材料不同，四個輪子裝配上存在誤差，輪子與場地之間摩擦力不一致，四個驅動電機存在機械差異等原因，使得機器人在加速過程中四個輪子加速不一致從而使機器人的實際位姿與期望位姿之間存在偏差。而且機器人運動速度越大，位姿的偏差也越大。在啟用的速度補償器后，將機器人整體進行閉環控制，通過模糊PD控制實時根據具體的工況對速度進行相應的補償修正并最終分配到四個輪子上，有利的改善了對軌跡跟隨的精度。

參考文獻

［1］LUCA C， ALESSANDRO D L， STEFANO I， Trajectory tracking control of a four-wheel differentially driven mobile robot ［C］ //International Conference on Robotics and Automation.Detroit， MI， USA: IEEE，1999: 2632-2638.

［2］ 熊蓉，張翮，褚健，等．四輪全方位移動機器人的建模和最優控制［J］．控制理論與應用，2006，23（1） ：93-98．

［3］ 宋海濤，張國良，王仕成，等．全向移動機器人最短時間控制［J］．電機與控制學報，2008，12（3）：337-342．

［4］ 陳啟軍，王月娟，陳輝堂．基于PD控制的機器人軌跡跟蹤性能研究與比較［J］．控制與決策，2003，18（1）：53-57．

［5］ 張涇周，楊偉靜，張安祥．模糊自適應PID控制的研究及應用仿真［J］．計算機仿真，2009，26（9）：132-135．

［6］ 許力．智能控制與智能系統［M］．北京：機械工業出版社，2007，2（1）：132-135．

注：本文中所涉及到的圖表、注解、公式等內容請以PDF格式閱讀原文