基于模糊自適應模型預測控制的移動機器人路徑跟蹤控制

黃正旭,周 坤,王斌銳

(中國計量大學 機電工程學院,浙江 杭州 310018)

目前,傳統(tǒng)垃圾收集方式存在垃圾外散、出現(xiàn)滿溢、異味外泄及長時間未清理造成二次環(huán)境污染,以及固定式垃圾收集點占用空間大等問題[1]。采用移動機器人實現(xiàn)定時定點投放、收集和運輸,對于降低垃圾分類收集成本、提高垃圾轉運效率、避免二次環(huán)境污染、減少垃圾收集空間占用具有重要意義。移動機器人可以定時定點進行垃圾收集工作,沿著指定的路徑進行移動,而要避免移動到不必要的區(qū)域,就需要具有良好的路徑跟蹤性能。路徑跟蹤的目的是確保機器人能夠被引導到期望路徑上,并且橫向誤差盡可能小[2]。其中,路徑跟蹤精度和轉向平穩(wěn)性是設計路徑跟蹤控制器的兩個關鍵指標。但是兩者并不是絕對的,要根據(jù)實際運行情況,合理選擇側重指標,能夠有效提高整體控制效果。

針對移動機器人路徑跟蹤問題,國內(nèi)外學者進行了大量研究,開發(fā)出多種路徑跟蹤控制器。Zheng等[3]、Mai等[4]構建PID控制器用于處理移動機器人的路徑跟蹤問題,但面對復雜工況時,較難實現(xiàn)對路徑跟蹤的最優(yōu)控制[5]。LQR控制器不考慮移動機器人運動學或者動力學約束,在一定工況下,容易使得機器人出現(xiàn)側偏失穩(wěn)現(xiàn)象[6]。純追蹤控制器很難為移動機器人選擇合適的前瞻距離,存在兩個連續(xù)控制周期下輸出不連續(xù)轉向角的問題。因此存在較大的航向誤差和橫向誤差,跟蹤不穩(wěn)定[7]。針對這些不足,模型預測控制(model predictive control, MPC)可以解決多變量、多約束的系統(tǒng)問題[8],因此在路徑跟蹤領域得到大量應用。

傳統(tǒng)的MPC控制器權重系數(shù)為常數(shù),當路徑曲率較大或者移動機器人遠離期望路徑時,路徑跟蹤精度或轉向平穩(wěn)性變差。文獻[9]中引入隨道路曲率變化的速度自適應調(diào)節(jié)算法,自動更新目標函數(shù)中的模型參數(shù),減小軌跡跟蹤的誤差。文獻[10]中MPC控制器根據(jù)路徑曲率調(diào)整采樣時間,預測時域隨著線速度和初始偏差的變化進行調(diào)整,以提高跟蹤精度。

常見的MPC控制器設計大多考慮跟蹤精度,而忽略了在某些特定場景下轉向平穩(wěn)性的優(yōu)化。本文針對此問題設計一種改進的MPC控制器,該控制器采用模糊控制器自適應地調(diào)整目標函數(shù)的權重系數(shù)。模糊控制器以移動機器人的橫向偏差、航向偏差和路徑曲率作為模糊控制器的輸入,以權重系數(shù)的調(diào)節(jié)因子作為模糊控制器的輸出。最后通過MATLAB仿真測試改進的MPC控制器路徑跟蹤性能。

1 移動機器人MPC控制器設計

模型預測控制使用當前和過去的偏差值,并且還使用預測模型來預測路徑跟蹤過程中的偏差值。當前最優(yōu)控制策略由滾動時域優(yōu)化確定,使未來一段時間內(nèi)被控變量與期望值的偏差最小,有效處理多約束問題。模型預測控制控制器一般由預測模型、目標函數(shù)、滾動優(yōu)化和反饋校正四部分組成[11]。

1.1 預測模型

本文所使用的移動機器人為四輪差速控制。機器人工作時,運動速度較慢,在運動過程中可以忽略側滑、側傾等影響穩(wěn)定性的因素。因此,利用運動學模型設計的路徑跟蹤控制器具有良好的路徑跟蹤性能。

假設移動機器人的本體是剛體且車輪不發(fā)生變形,機器人在平面上運動、不打滑,即車輪和地面之間為純滾動。在此理想條件下,以機器人中心為參考點,建立移動機器人的運動學模型,如圖1。圖中:XOY為全局坐標系,xoy為機器人坐標系,φ為機器人航向角。

圖1 移動機器人運動學模型Figure 1 Kinematic model of mobile robot

由圖1可知,在機器人中心處:

(1)

選取狀態(tài)量x=[xyφ]T,控制量u=[vw]T,簡化式得到機器人運動學模型為

(2)

式(2)中,v為機器人線速度,w為機器人角速度。

根據(jù)運動學模型可以將系統(tǒng)表示為

(3)

給定的期望路徑上的每一個期望路徑點,均符合機器人運動學模型,用r代表期望量,則期望路徑表示為

(4)

利用式(2)所建立的非線性機器人運動學模型預測機器人未來狀態(tài)量,需要占用大量的計算資源。因此需要對模型進行線性化處理,將式(4)在每一個期望路徑點按照一階泰勒級數(shù)展開,得到

(5)

用式(5)減去式(4),展開偏導數(shù),整理得到

(6)

將非線性系統(tǒng)轉換為狀態(tài)誤差為狀態(tài)量和控制量誤差為控制量的連續(xù)函數(shù),不能直接用于計算機計算處理操作,必須要轉換為離散化方程[12]。在此采用前向歐拉法對系統(tǒng)進行離散化,得到

(7)

整理得到

(8)

式(8)中,T為采樣時間,

(9)

新的機器人狀態(tài)空間表達式為

(10)

此時,系統(tǒng)控制量方程如下:

Y=ψξ(k)+θΔU(k)。

(11)

式(11)中,Nc為控制時域,Np為預測時域,

1.2 目標函數(shù)

在路徑跟蹤過程中,機器人的控制目標是盡可能靠近目標路徑,并獲得轉向的穩(wěn)定性,同時機器人的控制量變化也盡可能流暢,定義目標函數(shù)如下:

J=(Y-Yref)TQ(Y-Yref)+ΔUTRΔU+ρε2。

(12)

式(12)中,Yref為期望值,Q為狀態(tài)量權重系數(shù)矩陣,R為控制量權重系數(shù)矩陣,ε為松弛因子,ρ為松弛因子加權系數(shù)。

1.3 滾動優(yōu)化

MPC控制器的優(yōu)化控制是一個持續(xù)在線優(yōu)化過程,在系統(tǒng)的每一個采樣周期都會對目標函數(shù)進行一次優(yōu)化計算。將目標函數(shù)轉化為二次規(guī)劃問題進行求解。定義一個偏差e,e的表達式為

e=ψξ(k)-Yref。

(13)

聯(lián)立式(11)、式(12)和式(13),化簡為二次規(guī)劃標準型,得到

(14)

在目標函數(shù)的求解過程中,要考慮機器人的物理約束,將控制量和控制量的增量限制在合理的范圍內(nèi)[13],其約束條件如下:

(15)

式(15)中,ΔUmin為控制量增量的最小值,ΔUmax為控制量增量的最大值,Umin為控制量的最小值,Umax為控制量的最大值。

1.4 反饋校正

在每個控制周期內(nèi),根據(jù)約束條件,采用二次規(guī)劃器求解目標函數(shù),在控制時域內(nèi)計算出一系列最優(yōu)控制輸入增量

ΔU(k)=

[Δu(k),Δu(k+1),…,Δu(k+Nc-1)]T。

(16)

將式(16)的第一個元素作用于系統(tǒng),實際系統(tǒng)控制輸入量u(k)=u(k-1)+Δu(k)。

在下一時刻,重新計算最優(yōu)控制輸入,以達到對系統(tǒng)的持續(xù)控制[14]。

2 權重系數(shù)模糊自適應整定

在采用傳統(tǒng)固定權重系數(shù)MPC控制器的路徑跟蹤實驗中,當移動機器人遠離目標路徑時,由于過于追求跟蹤精度,機器人角速度在短時間內(nèi)急劇修正,從而影響機器人運動控制的穩(wěn)定性。而增大狀態(tài)量權重系數(shù)可以提高轉向平穩(wěn)性,但會帶來跟蹤精度下降的問題?；诖?設計了一種權重系數(shù)模糊自適應調(diào)節(jié)的MPC控制器,該控制器能夠根據(jù)機器人橫向誤差、航向誤差和路徑曲率,對目標函數(shù)的權重系數(shù)進行自適應整定,以保證跟蹤精度和轉向平穩(wěn)性。

根據(jù)模糊控制器設計,定義了3個模糊控制輸入,包括機器人橫向誤差eY、航向誤差eφ和路徑曲率ρ。定義兩個模糊控制輸出狀態(tài)量權重系數(shù)調(diào)節(jié)因子λQ和控制量權重系數(shù)調(diào)節(jié)因子λR?？紤]機器人在路徑跟蹤時實際工作狀態(tài),模糊控制器的橫向誤差和航向誤差輸入可以模糊化為5個模糊集:NB(負大)、NS(負小)、ZO(零)、PS(正小)、PB(正大)?？紤]路徑曲率輸入和權重系數(shù)輸出總是為非負的,路徑曲率輸入和權重系數(shù)輸出模糊化為四個模糊集:ZO(零)、PS(正小)、PM(正中)、PB(正大)[15]。根據(jù)實際控制經(jīng)驗,橫向誤差輸入和輸出選用高斯型隸屬度函數(shù)以提供更加準確的隸屬度值和更強的適應性,而對于航向誤差輸入和路徑曲率輸入需要更加靈敏,因此選用三角形隸屬度函數(shù)[16]。與模糊控制器相關的隸屬度函數(shù)如圖2。

圖2 模糊控制器輸入和輸出隸屬度函數(shù)Figure 2 Fuzzy controller input and output membership function

模糊規(guī)則庫包含考慮輸入與輸出之間相互關系的模糊規(guī)則,這些規(guī)則是根據(jù)權重系數(shù)對跟蹤效果的影響和實際控制經(jīng)驗所構造,如表1、表2。

表1 控制量權重系數(shù)調(diào)節(jié)因子λR模糊規(guī)則

表2 狀態(tài)量權重系數(shù)調(diào)節(jié)因子λQ模糊規(guī)則

圖3給出模糊規(guī)則下輸入與輸出關系的響應曲面。通過以上模糊規(guī)則,當機器人遠離期望路徑時,增加控制量的權重系數(shù),以確保機器人在接近期望路徑的過程中,保持轉向的穩(wěn)定性,犧牲一定的跟蹤精度。當機器人基本接近期望路徑時,在路徑曲率變化較大的情況下,通過增大狀態(tài)量的權重系數(shù)以減小橫向誤差,提高跟蹤精度。

圖3 模糊規(guī)則下輸入和輸出的響應曲面Figure 3 Response surface of input and output under fuzzy rules

Q、R矩陣權重系數(shù)調(diào)節(jié)算式為

Rw=Rw0×8λR,

(17)

Qx,y=Qx0,y0×4λQ。

(18)

式(17)和式(18)中,Rw0、Qx0,y0分別為原MPC控制器權重系數(shù),Rw、Qx,y分別為模糊自適應整定的MPC控制器權重系數(shù)。

3 仿真實驗

為了驗證改進后的MPC控制器的性能,在MATLAB上建立仿真系統(tǒng),比較傳統(tǒng)的MPC控制器與本文改進的MPC控制器?？紤]移動機器人的實際運行場景,跟蹤直線和雙移線組成的復合路徑[17]?？紤]移動機器人實際運行時狀態(tài),設定MPC控制器參數(shù)為Np=20,Nc=15,T=0.05 s,ε=10,機器人初始位置坐標為(0,3),初始線速度為1.0 m/s,初始角速度為0。期望路徑起始點為(0,2),初始位置機器人與期望路徑的橫向偏差為1 m,航向角偏差為0。相應的實驗結果如圖4。

圖4 路徑跟蹤仿真結果Figure 4 Path tracking simulation results

從圖4(b)中可知,傳統(tǒng)的MPC控制器在路徑跟蹤過程中盲目追求跟蹤精度,導致機器人在遠離目標路徑時,角速度在短時間內(nèi)出現(xiàn)高頻變化。改進后的MPC控制器通過模糊控制器對目標函數(shù)的權重系數(shù)進行自整定。當橫向誤差過大時,增加了角速度增量的權重系數(shù),以保證機器人平穩(wěn)地接近期望路徑,改進后的MPC控制器角速度變化更加平穩(wěn)。當然,這會導致部分跟蹤精度損失(橫向誤差最大差約0.08 m),從而導致平均橫向誤差的增大,如圖4(a)。改進的MPC控制器在接近預期路徑后,具有更小的橫向誤差,如圖4(c)。表3給出了控制器性能比較分析。

表3 控制器性能分析

在保證一定跟蹤精度的情況下,改進的MPC控制器保證了整個路徑跟蹤過程中機器人轉向的穩(wěn)定性。

4 結 語

針對移動機器人路徑跟蹤精度與轉向穩(wěn)定性矛盾的問題,設計了一種權重系數(shù)自適應整定的MPC控制器。該控制器在傳統(tǒng)MPC控制器的基礎上進行優(yōu)化,引入模糊控制器,使MPC控制器中的目標函數(shù)權重系數(shù)可以根據(jù)機器人橫向偏差、航向偏差和路徑曲率的變化進行自適應整定。

通過MATLAB仿真驗證該控制器的有效性。通過仿真測試,得出以下結論。

1)該控制器具有較高的跟蹤精度。

2)改進后的MPC控制器保證了在機器人遠離目標路徑時,轉向控制的平穩(wěn)性。

3)當機器人離期望路徑較近,在道路曲率變化較大時,減小機器人的橫向誤差。

目前的研究只針對固定路徑下的跟蹤效果,將路徑規(guī)劃與路徑跟蹤結合起來是實際應用中的一大研究重點。