基于改進模糊PID的全向搬運機器人路徑跟蹤控制研究

張 品 李長勇

(新疆大學機械工程學院，新疆 烏魯木齊 830049)

在食品工廠生產流程中，食品原料的轉運需要耗費很多時間，對食品生產的效率和工廠效益產生一定影響。采用自動化的搬運機器人[1-4]能有效降低食品原料轉運耗費的時間，并且節省大量人力物力。全向機器人[5]是一種動作靈活的移動機器人，全向機器人可以執行物品拿取，倉庫整理，地面清掃，來客識別等倉庫管理任務。路徑跟蹤控制是全向機器人應用的核心技術之一，路徑跟蹤研究主要是控制機器人運動的實際路徑與期望路徑的誤差，對機器人高效、穩定工作具有重要意義。

在移動機器人控制研究中，學者們經常使用PID方法[6-8]來控制機器人，普通PID方法控制效果不確定，參數無法自適應調整，整定困難。模糊邏輯控制[9-11]也經常被應用至機器人控制中，其具有穩定性高，抗擾動能力強等特點，但是控制效果依賴于專家經驗和控制規則優劣，缺少積分作用，在工作點附近穩態誤差較大。尹奇輝等[12]在設計控制器時加入模糊神經網絡，并用LQR對位姿偏差耦合關系解耦，加快了糾偏速度。陳軍章等[13]通過重新定義公式改善了人工魚群算法，并應用改進算法降低了跟蹤誤差。周仁芳等[14]通過分析實時位置和理想路徑，設計了去除時間因素的跟蹤控制器，取得比純追蹤更好的效果。朱欣華等[15]在理想路徑周圍設計一種帶有位置信息的場，能迅速糾正機器人的偏離。白國星等[16]通過減少控制步數優化非線性模型預測控制，提高了農業機器人控制精度。由上可知，PID控制、模糊控制、群智算法等在路徑跟蹤控制中改進或組合應用具有良好效果。

試驗擬基于一種三輪全向搬運機器人，利用裝置的角度、速度等傳感器作為機器人感知系統，提出一種改進模糊PID路徑跟蹤控制方法，引入論域放縮因子優化模糊控制器，同時加入反向學習策略和權重因子改進麻雀算法，以模糊PID的參數作為麻雀位置進行尋優。通過MATLAB軟件對改進麻雀算法進行測試，對改進模糊PID控制器進行模擬試驗，分析其跟蹤不同路徑的效果。

1 全向機器人運動學建模

1.1 模型說明

為了說明機器人的運動狀態和便于計算，作出以下假設：全向機器人行走輪與地面不產生相互滑動，機器人自身不產生俯仰傾覆等狀況。

1.2 全向機器人運動學模型

設全向機器人狀態變量和控制變量為

(1)

(2)

式中：

X——狀態方程；

U——控制方程；

x、y——機器人的位置坐標。

將機器人在行走過程中的狀態表示為圖1的示意圖，跟蹤過程中的參數由表1說明。對驅動輪的角速度和速度進行分析，得到以下坐標速度和驅動輪速度關系：

表1 全向機器人跟蹤參數

圖1 全向機器人跟蹤示意圖

(3)

設環境坐標系為XOY，機器人自身坐標系為xoy，在環境坐標系中的位姿表達為(XYθ)。

由兩坐標系關系可求得環境坐標系與自身坐標系間的表達式為：

(4)

式中：

Vwx、Vwy——機器人在環境坐標中的線速度，m/s；

Ww——機器人在環境坐標中的角速度，rad/s。

進一步推導驅動輪轉速與機器人在環境中的速度變換關系：

(5)

控制機器人的過程中，為了不時刻進行坐標系的轉換運算，假設自身坐標系與環境坐標系重合，則θ=0，式(5)可化簡為：

(6)

將矩陣展開化簡得：

(7)

將ψ=120°代入式(7)，故全向機器人的運動學模型為：

(8)

2 模糊PID控制器

2.1 PID控制器設計

PID控制器只以系統的輸出誤差為輸入，不依賴數學模型，設計方便，魯棒性高。通過組合底盤電機的不同動作，機器人就可以按照設計路線行走。電動機的轉速與其輸入電壓呈比例，設期望路徑與實際路徑的垂直距離為距離偏差h(t)，以距離偏差h(t)為PID控制器的輸入，以電機的輸入電壓u(t)為控制輸出。PID控制公式如下：

(9)

為了便于工程實現，確定2 ms為取樣時間，將樣點輸入控制器處理，表示為：

Δh(t)=h(t)-h(t-1)。

(10)

2.2 模糊控制器設計

設計一個模糊控制器。以機器人的位姿偏差為輸入，控制器的參數為輸出。在機器人行走的過程中，不斷將位姿偏差傳入，將控制參數傳出，調整機器人的路徑。位姿誤差分為距離偏差h，方向偏差θ。輸出參數為kp、ki、kd。

2.2.1 輸入輸出變量模糊化 依據控制參數選取規則，通過試驗初步測定，確定了變量的論域并劃分了模糊子集。

輸入的模糊子集和論域為：

(11)

輸出的模糊子集和論域為：

(12)

2.2.2 模糊規則設計 根據PID控制規律，當距離偏差或者方向偏差比較大時，應當盡快縮小偏差，縮短響應時間，此時kp值應該較大。當響應時間較小，但系統穩定時偏差仍然較大，應當適當增大ki的值，使響應值達到設定值。當響應時間較小，系統穩定時響應值在設定值附近大幅震蕩時，應該增大kd，以減小穩態誤差。

隸屬函數有多種類型，高斯函數過渡性好，控制器工作波動小，如圖2所示。

圖2 距離偏差隸屬度函數

根據試驗測試和PID參數規律，制定表2所示參數kp、ki、kd的模糊規則表。

表2 kp、ki、kd模糊規則表

工作過程中，將論域放縮因子α、β引入控制器，在機器人工作狀況發生變化時，對論域自我調節。放縮因子設計主要有兩種方式，一種是模糊推理型，另一種是函數型，函數型放縮因子參數選定困難，且隨著工況變化控制效果難以保證。研究選用模糊推理型放縮因子，基于專家經驗和前期試驗，模糊PID控制器兩個輸入參數e和ec的放縮因子α模糊子集為{Z,S,M,B,MB}，輸出參數kp、ki、kd的β模糊子集為{Z,S,M,B,MB}。

故輸入輸出的模糊論域X與Y記為：

(13)

α、β選取如表3。

表3 α、β模糊規則表

3 麻雀算法

3.1 基本麻雀算法

麻雀算法由Xue等[17]提出。該算法仿照麻雀種群的行為，將發現者位置更新公式定義為：

(14)

式中：

t——當前代數；

Tmax——最大迭代數；

φ——一個屬于(0,1]的隨機數；

E——警惕值，E∈[0,1]；

TS——安全閾值，TS∈[0.5,1]；

Q——服從正態分布的隨機數；

L——一行多列矩陣且元素是1。

當ETS，發現者發現了危險，會帶領種群飛往其他安全區域。

一部分麻雀隨著發現者行動作為跟隨者，在發現者找到食物時靠近爭奪食物，其中一小部分跟隨者因為食物太少，會飛往別處尋找食物，以補充能量。其位置更新公式如下：

(15)

式中：

Np——種群數；

Xp——目前所找到的最好食源；

Xworst——當前全局最差食源；

A——元素隨機賦值為1或-1的1×d矩陣；

A+——AT(AAT)-1。

還有一部分麻雀作為警戒者，時刻注意著周圍是否有捕食者出現，感知危險時會放棄食物，帶領種群飛往其他位置。其位置更新公式：

(16)

式中：

β——步長調節因子；

K——分布在[-1,1]的隨機數；

fi——當前個體適應度值；

fg、fw——全局最高、最低適應度值；

ε——極小常數。

3.2 改進麻雀算法

開始創造麻雀群時，麻雀個體隨機分布在尋優空間，分布不均勻會延長找到最優質食源的時間。若麻雀群均布在空間，靠近最優質食源的麻雀飛向食源的距離和時間更短。利用對立解和中間解綜合考慮創造原始麻雀群。

首先隨機生成n個麻雀個體，再通過個體的對立點生成新的麻雀個體，反向解生成公式：

(17)

式中：

ub、lb——尋優空間上界和下界。

種群的初始解和反向解也不能完全均布在搜索空間內，故再加入初始解和反向解的中間解來豐富種群的多樣性。中間解的表達式為：

(18)

種群總數量n*的表達式為：

(19)

依據個體的適應度，將總n*個體重新排列。取前n個麻雀作為開始尋優前的麻雀群，并舍棄排列在后面的麻雀。

由于跟隨者在察覺發現者找到食物后全部向發現者靠近，種群趨向單一區域，影響搜尋更優食源，容易圍繞次優食源盤旋。文中提出一種余弦權重因子ω，在尋優初期ω較小，一小部分跟隨者靠近找到食物的發現者，其他跟隨者保持原狀，避免陷入局部最優。尋優后期ω較大，大量跟隨者靠近發現者爭奪食物，加快收斂速度。

(20)

式中：

i——當前代數；

imax——最大迭代數；

m——調節系數。

圖3是ω在迭代中的變化趨勢。

圖3 ω遞增圖

則改進的跟隨者位置公式：

(21)

對改進的麻雀算法進行性能測試仿真試驗，參數為麻雀數量30，最大迭代數500次。分別選用單峰測試函數、多峰測試函數、固定維數測試函數進行測試。選用的函數如式(22)所示。

(22)

由于部分測試函數收斂快，對結果圖橫坐標進行了放大，仿真結果如圖4所示。

由圖4可知：在單峰函數測試下，改進麻雀算法收斂速度快于麻雀算法；在多峰函數測試下，改進的麻雀算法收斂速度比麻雀算法更快，且遇到次優作出反應迅速跳出，比原算法找到的食源更優；在固定維數函數測試下，改進麻雀算法先于原算法找到最優食源，并且尋優過程波動比麻雀算法小。

圖4 3種測試函數仿真結果

4 仿真試驗及結果分析

4.1 試驗過程

改進模糊PID控制器工作過程：

(1)創造原始麻雀群，并且通過策略提升質量。設置麻雀數量、最大迭代數、安全閾值、搜索空間等約束條件。

(2)依據式(14)、式(15)、式(21)，每個麻雀個體完成飛行動作，對自己的信息進行更新。

(3)適應度計算。設置ITAE準則為適應度函數，并將麻雀的3個維度值賦值給模糊PID控制器的3個參數。在simulink中進行仿真，將被控系統產生的穩態誤差代入ITAE函數計算適應度值。

(4)對比傳入麻雀算法和設定的適應度值，前者小于后者或迭代結束進行下一步，否則返回第二步。

(5)將找到的最好食源信息作為控制參數傳入仿真模型，得到結果。

仿真環境為MATLAB2018a/simulink，初始化麻雀群數量30，迭代數500代，仿真步長0.005 s，用改進模糊PID控制器搭建仿真模型。圖5是控制結構圖。

圖5 控制系統結構圖

4.2 結果及分析

為了對改進的模糊PID控制器控制全向機器人路徑跟蹤的效果進行驗證，先對控制器進行階躍仿真，然后選用直線、曲線路徑進行跟蹤仿真。

由圖6可知，兩種控制器最終都能讓系統收斂，并且上升時間差別不大。但是改進的模糊PID到達設定值的時間小得多，并且超調量很小，達到輸出穩態值后幾乎不再波動。

圖6 階躍信號響應圖

設機器人原位姿為(0,0，π/3)，理想路徑y=x+1。圖7、圖8為直線跟蹤圖和直線誤差圖。

圖7 直線跟蹤

圖8 直線誤差

由以上試驗結果可以得出，機器人跟蹤直線路徑時較為快速，并且到達期望路徑時間較短，到達期望路徑后波動很小，e(θ)最大值0.38，跟蹤誤差較小。

設機器人初始狀態分別為(0,2，π/3)，理想路徑y=1+2sin(0.9x)。圖9、圖10為曲線跟蹤圖和曲線誤差圖。

圖9 曲線跟蹤

圖10 曲線誤差

由試驗結果可知，跟蹤曲線路徑時，y和θ誤差有一定波動，但是很快也接近于0，機器人快速向目標路徑靠近，總體跟蹤效果較好，滿足機器人工作需求。

5 結論

針對在復雜食品原料倉庫中搬運機器人跟蹤控制問題，提出了一種應用于全向搬運機器人的路徑跟蹤控制方法，設計了模糊推理型論域放縮因子自動調節論域，通過綜合對立解和中間解的麻雀群生成方法提高了原始群質量，加入權重因子控制跟隨者靠近食源的比例。仿真結果顯示改進麻雀算法相比原算法在幾種測試函數下尋優精度和收斂速度有所提升，改進控制器在直線和曲線跟蹤時誤差較小，到達期望路徑較快，滿足全向搬運機器人工作要求。