基于灰狼算法優(yōu)化PID的機器人路徑跟蹤控制

文/朱敬旭輝 趙景波 薛秉鑫

自從機器人路徑跟蹤問題引起各界學(xué)者的注意，直到上個世紀九十年的之后，才開始了廣泛的研究。歷經(jīng)20多年的發(fā)展，眾多研究人員研發(fā)出了多種路徑跟蹤的控制方法。其中包括：PID控制、反步法、智能控制方法等多種控制方法。

本文采用融合動力學(xué)進行分析，首先建立了兩輪差速驅(qū)動移動機器人的運動學(xué)及動力學(xué)模型，并采用反步法設(shè)計了移動機器人路徑跟蹤控制規(guī)律，以此來計算控制量的期望值；然后提出了基于灰狼算法優(yōu)化的PID控制，采用灰狼優(yōu)化算法對PID控制參數(shù)進行自整定以提高控制系統(tǒng)的動態(tài)性能；最后仿真表明，利用本文提出的控制算法能夠有效穩(wěn)定地控制移動機器人完成路徑跟蹤任務(wù)。

1 移動機器人路徑跟蹤控制模型

移動機器人的經(jīng)典模型是兩輪差速驅(qū)動模型，其位置姿態(tài)圖如圖1所示。

根據(jù)圖1，建立兩輪差速驅(qū)動機器人的運動方程，如式（1）所示：

其中，(x,y)是機器人實際環(huán)境中的坐標位置，v是機器人在縱向方向上的速度，ω是機器人平面轉(zhuǎn)動的角速度，θ是機器人行進方向與坐標X軸的夾角，即方向角。根據(jù)機器人的位置(x,y,θ)與姿態(tài)(v,ω)解得機器人的位置姿態(tài)誤差微分形式為：

進一步得到：

計算可得控制規(guī)律：

其中c1，c2均為常數(shù)。

將式（4）代入式（3）得：

由Lyapunov穩(wěn)定性理論可知此系統(tǒng)具有穩(wěn)定性。

2 移動機器人路徑跟蹤控制器設(shè)計

在給定移動機器人的跟蹤路徑后，確定期望控制輸入v和ω，根據(jù)實時反饋的位姿(x,y,θ)，比較期望位姿和實際位姿得出地理坐標偏差，再變換到移動機器人坐標系（縱向x軸，以機器人中心左向垂直為y軸）。通過控制器控制輸入v和ω，逐步調(diào)整到期望位姿。機器人坐標系的偏差：

圖1：移動機器人位姿圖

圖2：狼群等級結(jié)構(gòu)圖

式中，(xd, yd, θd)為期望位姿，(xr, yr, θr)為機器人位姿，(xd-xr, yd-yr, θd-θr)為實際坐標位姿偏差。路徑跟蹤的最終目標是求解合適的(v,ω)滿足：

3 灰狼算法原理

灰狼群體中有著嚴格的層級制度，如圖2所示，其層級由高到低分為α、β、δ、ε四類，所屬層級越低的，其數(shù)量越多。低級狼受控于高級狼的命令，但高級狼也會受到低級狼的挑戰(zhàn)，如果低級狼挑戰(zhàn)成功將會取代高級狼的地位。當狼群中高等級的狼缺失時，會由β狼接替α狼的位置。狼群在追捕獵物時，由等級最高的狼帶領(lǐng)狼群對獵物進行有組織的圍攻，α、β、δ狼引領(lǐng)著ε狼逐漸向獵物靠近，逐步縮減對獵物的包圍范圍，然后伺機以極快的速度對獵物發(fā)起進攻。

表1：常規(guī)PID控制曲線路徑跟蹤實驗數(shù)據(jù)

表2：基于灰狼算法優(yōu)化的PID控制曲線路徑跟蹤實驗數(shù)據(jù)

GWO對狼群組織和狩獵方式的模擬，如圖3所示。將狼群分成α、β、δ、ε四組，由α、β、δ帶領(lǐng)ε向著目標搜索，在優(yōu)化過程中，不斷更新α、β、δ、ε的位置，見式（8）至式（12）。

其中，t為當前迭代次數(shù)，Pk表示獵物的位置，P表示灰狼的位置向量，C為隨機因子，r1、r2為[0,1]隨機數(shù)，max為最大迭代次數(shù)，a為收斂因子。D表示狼與獵物的距離。A控制搜索范圍，當|A|>1時，表示搜索范圍擴大，為全局搜索；當|A|<1時，表示搜索范圍收縮，為局部搜索。

因為α、β、δ距離目標的距離最近（即適應(yīng)度最高），所以可以利用α、β、δ所在位置信息引導(dǎo)ε判斷出目標的大體方位，對獵物進行逼近，見式（13）和式（14）：

其中，Pα代表α當前方位，Pβ代表β當前方位，Pδ代表δ當前方位。式（15）計算出ε的第t+1次迭代位置：

4 GWO優(yōu)化PID算法設(shè)計

4.1 基本思想

圖3：狼群位置更新原理

圖4：GWO優(yōu)化PID的系統(tǒng)結(jié)構(gòu)圖

將PID控制器的三個參數(shù)(KP, KI, KD)作為灰狼的位置向量，在給定的搜索空間內(nèi)，灰狼在適應(yīng)度值最優(yōu)的前三只狼α、β、δ的帶領(lǐng)下調(diào)整自己的位置，從而逐步逼近最優(yōu)目標。GWO優(yōu)化PID的系統(tǒng)結(jié)構(gòu)如圖4所示。GWO根據(jù)系統(tǒng)的運行狀態(tài)，不斷地調(diào)整PID的三個參數(shù)，從而使控制系統(tǒng)的性能指標和控制效果達到最優(yōu)。

4.2 適應(yīng)度函數(shù)的確定

PID控制器的優(yōu)化設(shè)計就是選擇合適的KP、KI和KD，使得系統(tǒng)各項性能指標和控制效果達到最優(yōu)。在控制系統(tǒng)中，常用的誤差性能指標有ISE、ISTE和ITAE，本文選用誤差絕對值時間積分函數(shù)ITAE指標作為目標函數(shù)，其定義為：

其中，t為時間，e(t)為系統(tǒng)給定值與系統(tǒng)輸出的偏差。

圖5：常規(guī)PID控制曲線跟蹤路徑及誤差圖

圖6：基于灰狼算法優(yōu)化的PID控制曲線跟蹤路徑及誤差圖

4.3 GWO優(yōu)化PID算法流程

采用GWO算法優(yōu)化PID參數(shù)的基本流程如下：

Step 1：給出PID三個參數(shù)的取值范圍，并在此范圍內(nèi)隨機初始化狼群；

Step 2：按照式（19）計算每只狼相應(yīng)的適應(yīng)度值；

Step 3：對每只狼按適應(yīng)度值排序，將適應(yīng)度值最優(yōu)的前三只狼分別設(shè)置為α、β、δ。

Step 4：根據(jù)式（8）-（11）、式（13）-（14）和式（16）-（18）更新所有ε狼的位置；

Step 5：更新參數(shù)a、A、C；

Step 6：判斷是否達到最大迭代次數(shù)max或其他結(jié)束條件（α的適應(yīng)度值大于或小于某一適應(yīng)度限值），若滿足則繼續(xù)執(zhí)行，否則，跳轉(zhuǎn)至Step 2；

Step 7：輸出最終α的位置，即得到最優(yōu)的PID參數(shù)。

5 路徑跟蹤仿真研究

為驗證本文提出的基于灰狼算法優(yōu)化的PID控制在移動機器人路徑跟蹤控制中的有效性和可行性，基于第一節(jié)中建立的兩輪差速驅(qū)動移動機器人運動學(xué)模型和控制模型，將本文所提控制算法用于移動機器人路徑跟蹤控制仿真實驗，并將該算法與常規(guī)PID控制進行了對比實驗?？紤]到在實際情況中路徑基本可以分解為直線路徑和曲線路徑，因此本文主要針對曲線路徑進行路徑跟蹤控制仿真研究。

在曲線路徑跟蹤仿真中，選擇常規(guī)PID控制參數(shù)為：KP=45，KI=0，KD=5，常規(guī)PID控制和基于灰狼算法優(yōu)化的PID控制曲線路徑跟蹤控制結(jié)果分別如圖5和圖6所示，兩種控制算法的相應(yīng)實驗數(shù)據(jù)分別如表1和表2所示。

以上仿真結(jié)果表明，本文所提控制算法在曲線路徑跟蹤控制中同樣取得了較為理想的控制效果，同時說明了該控制算法具有較好的魯棒性，在解決路徑跟蹤控制問題時具有一定的優(yōu)勢，可用于移動機器人路徑跟蹤任務(wù)。

6 總結(jié)

本文提出并使用的灰狼算法優(yōu)化的PID控制完成了對兩輪差速驅(qū)動移動機器人路徑跟蹤控制問題的研究。首先建立了移動機器人路徑跟蹤控制模型并證明了其穩(wěn)定性；然后詳細介紹了灰狼算法優(yōu)化的PID控制，并設(shè)計了移動機器人路徑跟蹤控制器；最后通過仿真對比實驗驗證了本文所提控制算法在移動機器人路徑跟蹤控制中的有效性和可行性。最后仿真結(jié)果表明利用本章提出的控制算法可以實現(xiàn)移動機器人對期望路徑的有效可靠跟蹤。