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

張?zhí)煊睿?彭憶強,2,3， 黃曉蓉,3， 孫樹磊,2,3， 何灼馀,3

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

0 引 言

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

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

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

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

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

1 平臺運動學(xué)建模

1.1 平臺結(jié)構(gòu)設(shè)計

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

圖1 平臺結(jié)構(gòu)示意圖

圖2 腳輪結(jié)構(gòu)示意圖

1）平臺運動保持水平，轉(zhuǎn)向軸垂直于水平面；

2）平臺各部件為剛體；

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

1.2 運動學(xué)建模

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

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

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

1.3 奇異位形分析

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

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

由轉(zhuǎn)向角逆運動學(xué)模型式（4）可得：

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

其中：

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

2 平臺動力學(xué)模型

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

圖3 平臺動力學(xué)建模流程

2.1 轉(zhuǎn)角動力學(xué)模型

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

圖4 力矩對比圖

2.2 轉(zhuǎn)向角動力學(xué)模型

對于腳輪轉(zhuǎn)向角動力學(xué)建模，令廣義變量q=θsi，平臺總能量在式（17）基礎(chǔ)上應(yīng)加上腳輪轉(zhuǎn)向所產(chǎn)生的能量，表達(dá)式如下：

對正運動學(xué)模型式（6）進(jìn)行求導(dǎo)，可得平臺的加速度，表達(dá)式如下：

3 控制器設(shè)計

3.1 基于逆動力學(xué)的雙閉環(huán)模糊PID控制器設(shè)計

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

圖5 基于逆動力學(xué)的雙閉環(huán)模糊PID控制策略結(jié)構(gòu)框圖

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

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

1）當(dāng)誤差和誤差率越大，參數(shù)kp應(yīng)當(dāng)逐漸增大，ki和kd應(yīng)當(dāng)逐漸減??；當(dāng)系統(tǒng)達(dá)到零點時，kp應(yīng)當(dāng)為最大，ki和kd應(yīng)當(dāng)為最?。?/p>

2）當(dāng)速度誤差絕對值較大時，為了提高系統(tǒng)響應(yīng)速度，應(yīng)該適當(dāng)增大kp值；如果全向移動平臺在完成特殊運動，如圓弧運動、轉(zhuǎn)彎運動等，應(yīng)當(dāng)適當(dāng)減小ki值，避免出現(xiàn)超調(diào)現(xiàn)象；

3）當(dāng)速度誤差和誤差率大小適中時，為了保證響應(yīng)速度的同時防止出現(xiàn)嚴(yán)重的超調(diào)現(xiàn)象，應(yīng)當(dāng)減小kp值，合理設(shè)置ki和kd值；

4）當(dāng)速度誤差絕對值較小時，為了提高移動平臺的穩(wěn)態(tài)性能，應(yīng)當(dāng)增大kp和ki的值；同時為了避免在初始值附近出現(xiàn)震蕩，應(yīng)當(dāng)增大kd值；

根據(jù)以上規(guī)則得到3個修正參數(shù)的模糊規(guī)則表，如表2、表3所示。

表2 模糊控制器1控制參數(shù)修正量

表3 模糊控制器2控制參數(shù)修正量

4 仿真分析

4.1 運動學(xué)和動力學(xué)仿真分析

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

表4 全向移動平臺結(jié)構(gòu)參數(shù)和幾何參數(shù)

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

表5 動態(tài)性能指標(biāo)對比結(jié)果

圖6 m =0.5時轉(zhuǎn)速曲線

圖7 實時變化時轉(zhuǎn)速曲線

圖8 動態(tài)耦合因子變化曲線

4.2 基于逆動力學(xué)的雙閉環(huán)模糊PID控制仿真分析

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

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

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

圖11 工況一自轉(zhuǎn)速度對比圖

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

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

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

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

圖15 工況二自轉(zhuǎn)速度對比圖

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

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

5 結(jié)束語

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

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