基于灰狼算法優化PID的機器人路徑跟蹤控制

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

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

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

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

移動機器人的經典模型是兩輪差速驅動模型，其位置姿態圖如圖1所示。

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

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

進一步得到：

計算可得控制規律：

其中c1，c2均為常數。

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

由Lyapunov穩定性理論可知此系統具有穩定性。

2 移動機器人路徑跟蹤控制器設計

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

圖1：移動機器人位姿圖

圖2：狼群等級結構圖

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

3 灰狼算法原理

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

表1：常規PID控制曲線路徑跟蹤實驗數據

表2：基于灰狼算法優化的PID控制曲線路徑跟蹤實驗數據

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

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

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

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

4 GWO優化PID算法設計

4.1 基本思想

圖3：狼群位置更新原理

圖4：GWO優化PID的系統結構圖

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

4.2 適應度函數的確定

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

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

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

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

4.3 GWO優化PID算法流程

采用GWO算法優化PID參數的基本流程如下：

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

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

Step 3：對每只狼按適應度值排序，將適應度值最優的前三只狼分別設置為α、β、δ。

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

Step 5：更新參數a、A、C；

Step 6：判斷是否達到最大迭代次數max或其他結束條件（α的適應度值大于或小于某一適應度限值），若滿足則繼續執行，否則，跳轉至Step 2；

Step 7：輸出最終α的位置，即得到最優的PID參數。

5 路徑跟蹤仿真研究

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

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

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

6 總結

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