腳輪式全向移動平臺的運動控制設計與仿真

張天宇， 彭憶強,2,3， 黃曉蓉,3， 孫樹磊,2,3， 何灼馀,3

(1. 西華大學汽車與交通學院，四川 成都 610039; 2. 汽車測控與安全四川省重點實驗室，四川 成都 610039;3. 四川省新能源汽車智能控制與仿真測試技術工程研究中心，四川 成都 610039)

0 引 言

腳輪式全向移動平臺在物流搬運、室內服務和外星探測等領域有著廣泛的應用，具有重要的研究價值[1]。腳輪可分為雙電機腳輪和單電機腳輪。雙電機腳輪中的兩個電機分別用于控制腳輪的轉速和轉向角，該結構在移動過程中會產生耦合運動，導致全向移動平臺屬于冗余系統、控制策略設計復雜且其運動精度低。電機腳輪具有機械結構簡單、性價比高以及其全驅動特性可有效降低控制策略設計難度等優點；因此，本文的研究對象為裝配有3個獨立驅動的單電機腳輪式全向移動平臺，主要分析其運動學模型、動力學模型以及控制策略。

運動學模型主要用于獲得跟蹤或到達目標的穩定運動控制規律，國內外學者采用幾何方法、D-H坐標變換法和輪心建模法等對輪式全向移動平臺運動學模型進行了大量研究。針對麥克納姆輪全向移動平臺，曾志雄[2]等采用幾何方法得到了運動學模型，并分析了其在狹窄過道的運動特性；葉長龍[3]等采用輪心建模法，建立具有偏心輪機構的運動學模型，以獲得最佳的結構設計參數，并對其運動軌跡誤差進行了分析。針對腳輪式全向移動平臺，Bruno[4]等首次用D-H坐標變換法，建立了單個腳輪的運動學模型；Ahmed[5]等采用矩陣求逆、偏導計算、雅可比矩陣等方式，建立了整體運動學模型。然而上述方法對于復雜多剛體平臺建模過程繁瑣，同時存在難以準確建立坐標系和確定其參數等問題。

輪式全向移動平臺的運動學模型中速度雅可比矩陣存在不可逆情況，會出現奇異位形問題，導致平臺自由度降低和靈活性變差[6]。針對腳輪式全向移動平臺奇異位形問題，Jea H C[7]等將兩個腳輪作為驅動輪，另一個腳輪作為轉向輪，從而擺脫奇異位形。Ahmed[5]等將每個腳輪轉速和轉向角速度進行耦合，利用雙輪差速原理擺脫奇異位形。以上兩種方法不能有效識別平臺是否處于奇異位形，導致平臺運動效率過低。Kim D[8]等采用單電機和雙電機腳輪搭配的工作方式擺脫奇異位形，但是該方法使平臺成為冗余驅動系統，增大了控制策略設計難度。

輪式全向移動平臺要達到快速且高精度的速度跟蹤，必須建立平臺的動力學模型，以運動學和動力學模型為基礎設計控制策略。針對腳輪式全向移動平臺，Ahmed[5]等采用PID控制器對腳輪轉速進行控制，實現了參考速度的跟蹤；Shi C S[9]等采用PID控制器實現了平臺點到點跟蹤控制。PID控制器其本質是一個線性控制器，只能在系統的某個平衡點或其鄰域內有效，對復雜非線性平臺的控制效果不佳。針對其他輪式移動平臺，Soudbakhsh D[10]等采用線性最優法和滑模變結構控制法對平臺進行軌跡跟蹤控制；倪洪杰[11]等利用預演信息設計預演控制策略，調節平臺驅動電機電壓使其快速跟蹤參考軌跡；孫棣華[12]等構造出一種自適應反步控制器，實現外部環境參數變化時平臺良好的路徑跟蹤功能。上述設計的策略主要是基于現代控制理論，所設計方法均要求系統具有精確的數學模型，然而平臺動力學模型中存在較多非線性項，證明系統穩定性時計算量大，導致無法獲得控制率。

針對上述問題，本文采用正交分解法建立運動學模型，引入耦合因子動態調節平臺避免奇異位形，采用拉格朗日方法建立平臺的動力學模型且去掉向心力和科式力引起的力矩項得到其簡化模型，設計基于逆動力學的雙閉環模糊PID控制策略，仿真結果表明平臺能快速準確地跟蹤參考速度。

1 平臺運動學建模

1.1 平臺結構設計

圖1表示平臺結構，包括3個腳輪和1個車身。圖2表示每個腳輪具有3個自由度，分別是繞輪心A的角速度、繞轉向軸B的轉向角速度、繞輪地接觸點C旋轉的角速度。腳輪的位置由結構參數、、、和時變參數、確定。其中為轉向點與平臺中點連線長度，為轉向點與平臺中點連線相對于X軸的夾角，為轉向點與輪心的偏置距離，為腳輪半徑，為腳輪繞輪心旋轉角度，為腳輪繞轉向軸旋轉角度。平臺運動過程中滿足如下假設：

圖1 平臺結構示意圖

圖2 腳輪結構示意圖

1）平臺運動保持水平，轉向軸垂直于水平面；

2）平臺各部件為剛體；

3）腳輪與地面沒有相對滑動，滿足純滾動條件。

1.2 運動學建模

表1 腳輪1速度正交分解表

由純滾動約束條件可知，平行方向上的速度分量必須由腳輪的轉速抵消平衡，可得滾動約束；垂直方向上的速度分量必須由腳輪轉向角速度抵消平衡，可得滑動約束，分別表示如下：

同理可得腳輪2和腳輪3的滾動約束和滑動約束，將3個腳輪的滾動約束和滑動約束進行耦合可得：

1.3 奇異位形分析

對逆運動學模型式（4）進行分析：1）當θs1=θs2= θs3=kπ時（k=1，2，3, ······,下同），平臺無法在X軸方向運動；2）當 θs1= θs2= θs3= π/2+kπ時，平臺無法在Y方向運動；3）當 θs1= θs2= θs3= π/4+kπ時，平臺無法在與X軸夾角為的方向上運動。因此，當3個腳輪的初始轉向角度相互平行時，平臺在垂直于輪軸的方向上將喪失一個自由度，失去全向移動能力，平臺處于奇異位形。

針對該問題，本文采用耦合逆運動學方法解決奇異位形問題，基本思路是將兩個腳輪角速度進行耦合，虛擬驅動第3個腳輪轉向運動。由式（5）可得腳輪3轉向角正運動學模型：

由轉向角逆運動學模型式（4）可得：

將式（9）、（10）和（11）耦合得到腳輪 1和腳輪2的耦合運動學模型。同理可得腳輪1和腳輪3、腳輪2和腳輪3的耦合運動學模型，寫成矩陣如下式所示：

其中：

由上述過程可知每個腳輪參與了兩次耦合，為了消除多次耦合產生的誤差，并進一步提高計算效率，需要判定平臺是否處于奇異位形，從而決定式（12）中的耦合項是否參與運算。假設平臺期望速度為、，當前實際速度為、，定義當前實際速度和最終期望速度夾角Ψ =|δ?β|，式中δ=arctan表示當前速度方向和X軸正向的夾角；β=arctan(vyr/vxr)，表示期望速度方向和X軸正向的夾角。在運動過程中平臺實際速度、和夾角時刻變化，而期望速度、的夾角保持固定，所以是關于實際速度、的函數，且進一步分析可知：當Ψ =π/2時，此時平臺完全處于奇異位形，不具有全向移動能力，需要耦合項參與運算，當或者時，平臺完全遠離奇異位形，具有全向移動能力，不需要耦合項參與運算。為了滿足以上要求，設置動態耦合調節因子：

2 平臺動力學模型

建立平臺動力學模型可用牛頓歐拉方程或拉格朗日法。相較于牛頓歐拉方程，拉格朗日法只需要知道平臺的速度而不涉及內力，更簡潔高效。所以，本文采用拉格朗日法建立平臺動力學模型。已知平臺的線速度和角速度，可得拉格朗日算子，拉格朗日算子對驅動變量求偏導，即可得到拉格朗日動力學方程，建模流程如圖3所示。

圖3 平臺動力學建模流程

2.1 轉角動力學模型

對上述轉角動力學模型式（18）進行Matlab/Simulink仿真分析。為了保證結果的有效性和廣泛性，取任意隨機速度作為參考速度，得到3個腳輪力矩如圖4所示。分析可知，第二部分力矩要遠小于第一部分力矩，其原因是在運動過程中，為了保持穩定性和高效性，平臺轉向過程較平穩，腳輪轉向角速度很小，因此第二部分力矩可以忽略，以消除轉向角速度對平臺的影響，且不會改變計算結果，簡化后的表達如下式所示：

圖4 力矩對比圖

2.2 轉向角動力學模型

對于腳輪轉向角動力學建模，令廣義變量q=θsi，平臺總能量在式（17）基礎上應加上腳輪轉向所產生的能量，表達式如下：

對正運動學模型式（6）進行求導，可得平臺的加速度，表達式如下：

3 控制器設計

3.1 基于逆動力學的雙閉環模糊PID控制器設計

對于速度控制器而言，經典控制方法是PID控制，但是PID控制的3個參數的整定是一個關鍵的問題。針對精確的數學模型，采用不同的參數直接影響被控信號的超調量和調節時間等。在實際的工程應用中常采用試湊的方法來整定參數，這為設計帶來了諸多不便[13]，而模糊PID控制器能夠根據一定的模糊規則對PID參數進行優化，克服PID控制器無法動態調參的缺點。為了實現對平臺在非線性及不確定性條件下的速度精確跟蹤問題，根據模糊PID控制原理提出平臺仿真控制策略如圖5所示。當指定平臺的期望速度后，由式（2）和（3）逆運動學模型可得腳輪當前轉速和轉向角度；經模糊PID控制器2調節后得到平臺的驅動力矩，然后輸入給式（19）、（22）和（24）的逆動力學模型；最后由平臺得到實際速度，經過反饋后和期望速度共同輸入給式（13）得到動態耦合因子值 ，動態調節運動學模型中耦合項的比重。上述仿真控制系統中包含兩個控制環：外環實現精確的全局速度跟蹤，內環穩定控制腳輪轉速。

圖5 基于逆動力學的雙閉環模糊PID控制策略結構框圖

3.2 輸入/輸出變量的模糊處理

選取Matlab/Simulink調試的PID參數作為模糊PID控制器的初始參數，控制器1初始參數：kp=0.1，ki=0.8，kd=–1.9，控制器 2初始參數：kp=1.1，ki=0.6，kd=0.3。速度誤差E和速度誤差變化率Ec的基本論域為[–0.2，0.2]，修正參數、、的基本論域為[–5，5]。速度誤差E及變化率Ec作為其輸入，通過模糊推理確定PID控制器3個參數。根據文獻[14]提出的模糊控制規則主要思想，結合平臺的控制要求，通過查閱相關資料[15]，模糊規則選取應當遵循以下原則：

1）當誤差和誤差率越大，參數kp應當逐漸增大，ki和kd應當逐漸減小；當系統達到零點時，kp應當為最大，ki和kd應當為最小；

2）當速度誤差絕對值較大時，為了提高系統響應速度，應該適當增大kp值；如果全向移動平臺在完成特殊運動，如圓弧運動、轉彎運動等，應當適當減小ki值，避免出現超調現象；

3）當速度誤差和誤差率大小適中時，為了保證響應速度的同時防止出現嚴重的超調現象，應當減小kp值，合理設置ki和kd值；

4）當速度誤差絕對值較小時，為了提高移動平臺的穩態性能，應當增大kp和ki的值；同時為了避免在初始值附近出現震蕩，應當增大kd值；

根據以上規則得到3個修正參數的模糊規則表，如表2、表3所示。

表2 模糊控制器1控制參數修正量

表3 模糊控制器2控制參數修正量

4 仿真分析

4.1 運動學和動力學仿真分析

為了驗證所提動態耦合因子方法擺脫奇異位形的有效性，根據運動學和動力學模型，當平臺在時刻處于奇異位形的工況時，進行Matlab/Simulink仿真，平臺的結構參數如表4所示。設置平臺初始位姿為（單位：rad，下同）；指定平臺的參考速度為（單位：線速度m/s、角速度rad/s，下同）。

表4 全向移動平臺結構參數和幾何參數

圖6是耦合因子取最大值0.5得到的腳輪轉速；圖7是采用動態耦合因子得到的腳輪轉速。對比可知：1）當平臺處于奇異位形時，腳輪3保持不動，腳輪1和腳輪2迅速反向轉動，帶動平臺順時針扭動一個微小角度，此時平臺等效于雙輪差速模型。當平臺擺脫奇異位形時，腳輪3開始轉動，并迅速和腳輪1、腳輪2達到穩定轉速，帶動平臺實現期望運動。2）當采用動態耦合因子后，系統能夠根據當前位形和奇異位形實時調整耦合項的比重，更合理地控制3個腳輪的轉速，使系統的動態性能指標發生顯著變化，見表5。

表5 動態性能指標對比結果

圖6 m =0.5時轉速曲線

圖7 實時變化時轉速曲線

圖8 動態耦合因子變化曲線

4.2 基于逆動力學的雙閉環模糊PID控制仿真分析

為了驗證所提控制策略的正確性和有效性，本小節將其與文獻[7]設計的PID控制策略進行比較分析；對于平臺是否處于奇異位形，仿真設置了兩種工況，所得分析結果如圖9～ 圖16所示。

圖9 工況一橫向速度對比圖

圖10 工況一縱向速度對比圖

圖11 工況一自轉速度對比圖

圖12表示平臺的運動軌跡，從圖可知，所提策略得到的最大縱向位移偏移量是–0.003 m，而文獻[7]的最大偏移量是–0.024 m；同時，當系統達到穩定狀態時，文獻[7]得到的實際軌跡與期望軌跡始終存在–0.005 m左右的誤差，而所提策略能使平臺實際軌跡迅速收斂到期望軌跡且誤差保持在–0.0001 m 左右。

圖12 工況一平臺質心的運動軌跡對比圖

圖13 工況二橫向速度對比圖

圖14 工況二縱向速度對比圖

圖15 工況二自轉速度對比圖

圖16表示平臺的運動軌跡，從圖可知，所提策略得到的最大偏移量是0.002 m，而文獻[7]的最大偏移量是0.3 m；平臺逐漸收斂于穩定狀態時，文獻[7]得到的實際軌跡在PID控制作用下出現了偏差糾正過度情況，誤差從–0.07 m一直增加到0.3 m，且呈現繼續增加趨勢；而本文所提策略能使平臺實際軌跡迅速收斂到期望軌跡并一直保持在0.0005 m的誤差范圍。

圖16 工況二運動軌跡對比圖

5 結束語

本文對腳輪式全向移動平臺運動控制問題進行了研究。采用簡潔高效的正交分解法，根據純滾動約束條件得到了平臺的運動學模型。針對運動學模型速度雅可比矩陣不可逆導致自由度降低的奇異位形問題，通過在運動學模型的基礎上增加由轉角運動學和轉向角運動學耦合得到的耦合項，引入耦合因子動態調節耦合項的比重。采用拉格朗日法建立平臺的動力學模型并對其進行相應簡化；基于上述模型，提出了基于逆動力學的雙閉環模糊PID控制策略。

通過Matlab仿真得到的結果表明：設計的動態耦合項能使平臺快速且有效擺脫奇異位形，恢復全向移動能力；對動力學模型分析可知平臺在運動過程中轉向角速度小、轉向過程平穩，可以忽略其對平臺的影響，簡化后的動力學模型消除了轉向角速度對平臺能耗的影響，表明所提簡化方法是正確且有效的；同時，所提控制方法可實現平臺精確快速的跟蹤參考速度，具有良好的穩定性和魯棒性。上述研究可為腳輪式全向移動平臺下一步實際搭建工作提供理論支撐。