趙 韓 尹曉紅 吳焱明

合肥工業(yè)大學(xué),合肥,230009

非完整約束AGV軌跡跟蹤的非線性預(yù)測(cè)控制

趙 韓 尹曉紅 吳焱明

合肥工業(yè)大學(xué),合肥,230009

針對(duì)存在非完整約束和控制輸入約束的三輪AGV模型,研究其非線性模型預(yù)測(cè)控制策略,提出了一種跟蹤與鎮(zhèn)定統(tǒng)一控制算法。首先由模型預(yù)測(cè)控制原理產(chǎn)生一個(gè)優(yōu)化控制器,接著設(shè)計(jì)終端控制器及不變終端域來保證系統(tǒng)的鎮(zhèn)定,然后設(shè)計(jì)狀態(tài)觀測(cè)器以進(jìn)一步提高跟蹤精度,最后設(shè)計(jì)避障控制模塊來完善整個(gè)控制器功能。該控制算法具有一般性,對(duì)所有AGV及其他輪式移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型都適用。計(jì)算機(jī)仿真結(jié)果驗(yàn)證了該控制算法的正確性和有效性。

AGV;模型預(yù)測(cè)控制;鎮(zhèn)定;觀測(cè)器;軌跡跟蹤

0 引言

近年來,自動(dòng)引導(dǎo)車(automatic guided vehicle,AGV)作為非完整約束輪式移動(dòng)機(jī)器人(wheeled mobile robot,WMR)的一種,由于實(shí)際應(yīng)用的廣泛性,其控制問題也得到了國內(nèi)外學(xué)者的關(guān)注。機(jī)器人的運(yùn)動(dòng)控制大致可以分為鎮(zhèn)定控制與軌跡跟蹤控制兩類。作為一種具有非完整約束的典型系統(tǒng),WMR不滿足反饋穩(wěn)定的Brockett定理的必要條件,因此不能通過連續(xù)時(shí)不變狀態(tài)反饋控制方法實(shí)現(xiàn)機(jī)器人系統(tǒng)的鎮(zhèn)定,故從理論而言,設(shè)計(jì)鎮(zhèn)定控制器更具有挑戰(zhàn)性。但從實(shí)踐意義上來說,軌跡跟蹤更具有實(shí)際應(yīng)用價(jià)值,因?yàn)榧词箤?duì)于鎮(zhèn)定控制任務(wù),機(jī)器人也要沿某條可行軌跡運(yùn)動(dòng)到目標(biāo)點(diǎn)。為了解決WMR的運(yùn)動(dòng)控制問題,人們已經(jīng)提出各種控制方法,包括滑?？刂品椒╗1]、反演控制方法[2]等在內(nèi)的非線性控制方法,自適應(yīng)控制方法[3],神經(jīng)網(wǎng)絡(luò)控制方法[4],時(shí)變反饋控制法[5],非連續(xù)反饋控制法[6]等,取得了較好的控制效果。然而,上述傳統(tǒng)方法的可調(diào)參數(shù)與控制性能之間的關(guān)系不是很直觀,參數(shù)整定很費(fèi)力,且在考慮實(shí)際存在控制輸入約束或狀態(tài)約束時(shí),比較難處理。而模型預(yù)測(cè)控制(model predictive control,M PC)可以克服以上缺陷,模型預(yù)測(cè)控制利用求懲罰函數(shù)的最小化來確定最優(yōu)控制輸入,參數(shù)調(diào)整容易,且狀態(tài)約束及控制輸入約束可以在優(yōu)化過程中直接考慮在內(nèi),簡(jiǎn)單明了。模型預(yù)測(cè)控制亦稱為滾動(dòng)優(yōu)化控制(receding horizon contro l,RHC),是近年來發(fā)展起來的一類新型計(jì)算機(jī)控制算法,由于它采用模型預(yù)測(cè)、滾動(dòng)優(yōu)化和反饋校正等控制策略,且其處理約束能力強(qiáng),因而控制效果好,已成功應(yīng)用于石油、化工、機(jī)械等比較復(fù)雜且模型不容易精確建立的工業(yè)生產(chǎn)過程。但是,由于源于工程控制的預(yù)測(cè)控制在設(shè)計(jì)時(shí)沒有在理論上考慮系統(tǒng)的閉環(huán)鎮(zhèn)定性,預(yù)測(cè)控制是在線反復(fù)求解開環(huán)優(yōu)化問題,而開環(huán)最優(yōu)不能保證閉環(huán)鎮(zhèn)定,因此,如何保證預(yù)測(cè)控制的鎮(zhèn)定性成了一個(gè)關(guān)鍵問題。

本文在研究AGV系統(tǒng)的非線性模型預(yù)測(cè)控制問題時(shí),在基本的預(yù)測(cè)控制原理基礎(chǔ)上著重考慮了預(yù)測(cè)控制的鎮(zhèn)定性,設(shè)計(jì)了終端控制器和終端域,提出一種跟蹤與鎮(zhèn)定統(tǒng)一的控制策略。同時(shí),考慮到運(yùn)動(dòng)過程中總會(huì)存在噪聲干擾,使得AGV自定位不夠準(zhǔn)確,影響軌跡跟蹤,本文用一個(gè)簡(jiǎn)單的狀態(tài)觀測(cè)器對(duì)AGV狀態(tài)進(jìn)行估計(jì),從而進(jìn)一步提高控制精度。最后本文還增加了一個(gè)避障功能模塊,以此來進(jìn)一步完善整個(gè)控制器功能。

1 AGV模型和預(yù)測(cè)控制模型

以具有非完整約束的三輪移動(dòng)機(jī)器人小車作為控制對(duì)象,其前輪既是轉(zhuǎn)向輪又是驅(qū)動(dòng)輪,結(jié)構(gòu)如圖1所示。

假設(shè)小車當(dāng)前實(shí)際位姿為X=(x,y,θ)T,任意給定的參考位姿為X r=(x r,y r,θr)T,位姿誤差X e=(x e,y e,θe)T;當(dāng)前實(shí)際速度 U=(v,ω)T,參考速度Ur=(vr,ωr)T,其中,vr為參考線速度,ωr為參考角速度。圖1中,D為后輪軸與中心軸之間的交點(diǎn),M為小車質(zhì)心。非完整約束意味著小車的輪子只有純滾動(dòng)而無滑動(dòng),即,其中d為點(diǎn)D與點(diǎn)M 之間的距離。移動(dòng)機(jī)器人小車的運(yùn)動(dòng)學(xué)方程如下:

圖1 三輪AGV結(jié)構(gòu)示意圖

2 軌跡跟蹤的模型預(yù)測(cè)控制算法設(shè)計(jì)

模型預(yù)測(cè)控制是一種基于模型的控制,其機(jī)理可以描述為:在每個(gè)采樣時(shí)刻,預(yù)測(cè)模型根據(jù)獲得的當(dāng)前系統(tǒng)實(shí)際狀態(tài)信息,在線求解一個(gè)有限時(shí)域開環(huán)最優(yōu)控制問題,將所得到的控制序列的第一項(xiàng)應(yīng)用于被控系統(tǒng),剩余的控制輸入丟棄,直至下一采樣周期。在下一采樣時(shí)刻測(cè)得系統(tǒng)的實(shí)際狀態(tài),重復(fù)求解優(yōu)化過程,即所謂的滾動(dòng)優(yōu)化。

然而,上述開環(huán)的過程并不能保證被控系統(tǒng)的鎮(zhèn)定。為了解決這個(gè)問題,可以在求解最優(yōu)問題時(shí)在懲罰函數(shù)中加入一個(gè)終端懲罰函數(shù)項(xiàng)以及終端約束,并引入一個(gè)終端域的概念。本文的控制思想是把整個(gè)控制過程分成兩個(gè)控制階段:首先,在有限時(shí)域內(nèi)求解最優(yōu)控制問題,將獲得的最優(yōu)控制輸入到系統(tǒng),驅(qū)動(dòng)系統(tǒng)使其進(jìn)入到終端域;然后,利用一個(gè)終端線性反饋控制器將系統(tǒng)控制到平衡點(diǎn)。預(yù)測(cè)控制過程如圖2所示,圖中,t為當(dāng)前時(shí)刻,Tp為預(yù)測(cè)時(shí)域。

圖2 預(yù)測(cè)控制過程示意圖

因此,本文的非線性模型預(yù)測(cè)跟蹤控制思路是:系統(tǒng)給定AGV的參考速度U r和參考位姿X r,它們可以由路徑規(guī)劃算法求得;期望位姿Xr與由狀態(tài)觀測(cè)器觀測(cè)得到的AGV當(dāng)前位姿進(jìn)行比較得到AGV 全局位姿誤差矢量通過MPC控制器進(jìn)行優(yōu)化和終端鎮(zhèn)定控制,輸出得到最優(yōu)控制(t,X(t)),再根據(jù)式(4),結(jié)合參考速度Ur,即可得到 AGV最優(yōu)速度控制信號(hào)U*;將U*=(v*,ω*)T輸入系統(tǒng)對(duì) AGV進(jìn)行控制,最后在狀態(tài)反饋時(shí)再通過一個(gè)狀態(tài)觀測(cè)器來提高AGV狀態(tài)反饋精度,從而提高整個(gè)控制系統(tǒng)性能,實(shí)現(xiàn)系統(tǒng)全局漸進(jìn)穩(wěn)定。圖3為AGV非線性模型預(yù)測(cè)控制框圖。

2.1 MPC優(yōu)化控制器設(shè)計(jì)

考慮由以下微分方程來描述的非線性系統(tǒng):

此外,由于觀測(cè)誤差直接影響控制效果,為了能更加準(zhǔn)確地估計(jì)AGV的當(dāng)前位姿,本文為其設(shè)計(jì)了狀態(tài)觀測(cè)器。同時(shí),針對(duì)有時(shí)候參考軌跡上會(huì)出現(xiàn)意外的障礙的情況,為了讓整個(gè)控制功能更加完整,本文還考慮了避障功能模塊。

圖3 控制框圖

選取懲罰函數(shù)為預(yù)測(cè)狀態(tài)及相應(yīng)控制輸入的二次函數(shù):

2.2 MPC終端控制器設(shè)計(jì)

為了表示區(qū)別,我們用X eT=(x eT,y eT,θeT)T代表終端狀態(tài)下的位姿誤差,其中,xeT為終端狀態(tài)下的x方向位置誤差;yeT為終端狀態(tài)下的y方向位置誤差;θeT為終端狀態(tài)下的θ姿態(tài)角誤差。

綜上,結(jié)合控制框圖3,控制算法步驟可以概括如下:

(2)設(shè)δ為采樣時(shí)間,在下一采樣周期t+δ時(shí)刻,根據(jù)反饋得到的此刻的系統(tǒng)實(shí)際狀態(tài)信息(t+δ)重新解決最優(yōu)化問題(式(7)),獲得此刻的最優(yōu)控制輸入(t,(t+δ)),則直接取該最優(yōu)控制輸入作為系統(tǒng)控制輸入。如果此刻還在預(yù)測(cè)時(shí)域T p內(nèi)(即t+δ≤τ≤t+T p),則直接取該最優(yōu)控制輸入作為系統(tǒng)控制輸入;否則,如果此刻已經(jīng)超過預(yù)測(cè)時(shí)域T p(即t+T p≤τ≤t+Tp+δ),則上面終端域設(shè)計(jì)的終端控制器啟用,取上述終端控制器的作為系統(tǒng)控制輸入。即輸入系統(tǒng)的控制輸入矢量應(yīng)為

(3)重復(fù)上面的步驟直至達(dá)到滿意的控制效果。

2.3 狀態(tài)觀測(cè)器設(shè)計(jì)

上述模型預(yù)測(cè)算法需要在每個(gè)采樣時(shí)刻由狀態(tài)觀測(cè)器觀測(cè)系統(tǒng)狀態(tài)并據(jù)此更新優(yōu)化問題的初始條件,從而滾動(dòng)優(yōu)化計(jì)算當(dāng)前時(shí)刻的預(yù)測(cè)控制量。顯而易見,優(yōu)化問題的最優(yōu)解是關(guān)于觀測(cè)狀態(tài)的函數(shù),在這里我們引入一個(gè)優(yōu)化控制器增益矩陣K的概念。

2.4 避障模塊設(shè)計(jì)

假設(shè)AGV小車上部環(huán)繞安裝有若干個(gè)紅外傳感器,它們能隨時(shí)隨地檢測(cè)到前方的障礙物信息并反饋給小車控制器,以便能及時(shí)調(diào)整軌跡跟蹤控制方案使之避過障礙物而不發(fā)生碰撞。對(duì)于移動(dòng)障礙物,可以利用預(yù)測(cè)控制的方法來預(yù)測(cè)下一個(gè)T p的障礙物位姿信息以及障礙物移動(dòng)速度,并通過計(jì)算類似上面的優(yōu)化問題來獲得系統(tǒng)的最優(yōu)控制輸入。設(shè)小車共環(huán)繞安裝有Ns個(gè)紅外傳感器,第 j個(gè)的傳感器測(cè)得的障礙物信息為[θob,j d ob,j](j=1,2,…,N),其中,θob,j為第j個(gè)

傳感器與前方障礙物的夾角;dob,j為第j個(gè)傳感器距離前方障礙物的直線距離。

3 仿真研究

為了驗(yàn)證本文所提出的控制算法的正確性和有效性,在MATLAB環(huán)境下編程,對(duì)AGV進(jìn)行仿真計(jì)算和性能分析比較。

AGV期望軌跡取以(0,0)為圓心,以1為半徑的圓,參考速度為v r=0.2m/s,ωr=0.2m/s。AGV小車實(shí)際初始值取x(0)=1.2m,y r(0)=1.2m,y r(0)=-0.3m,θ(0)=2π/3,v(0)=0.4m/s,ω(0)=0.3rad/s。

(1)在僅使用基本的模型預(yù)測(cè)跟蹤控制率而不使用所設(shè)計(jì)的終端控制器的情況下,對(duì)AGV小車進(jìn)行跟蹤控制仿真,取基本模型預(yù)測(cè)控制器性能加權(quán)矩陣Q=diag(0.5,0.5,0.5),R=diag(0.1,0.1),預(yù)測(cè)時(shí)域與控制時(shí)域取為 Tp=T c=1s,采樣周期取為δ=0.1s,即向前預(yù)測(cè)步數(shù)為N=10。跟蹤效果如圖4所示。

(2)結(jié)合所設(shè)計(jì)的預(yù)測(cè)模型終端控制器和狀態(tài)觀測(cè)器對(duì)AGV進(jìn)行仿真試驗(yàn),取模型預(yù)測(cè)控制器性能加權(quán)矩陣P=diag(1,1,1);性能系數(shù)取為α=2,β=1。預(yù)測(cè)時(shí)域T p、控制時(shí)域T c和采樣周期δ參數(shù)設(shè)置同上。圖5為AGV小車的跟蹤效果圖。

圖4 不使用所設(shè)計(jì)的終端控制器的情況下的仿真結(jié)果

此外,為了驗(yàn)證所設(shè)計(jì)避障模塊的有效性,對(duì)期望軌跡為簡(jiǎn)單的直線運(yùn)動(dòng)的AGV進(jìn)行避障仿真試驗(yàn)。設(shè)期望軌跡上有四個(gè)障礙,避障控制器的系數(shù)取為kob=1.5,c1=0.1,c2=10,圖6是小車躲避靜態(tài)障礙物的效果圖。

比較圖4和圖5仿真結(jié)果可見,由于本文所設(shè)計(jì)的控制律在設(shè)計(jì)時(shí)就考慮控制約束和終端鎮(zhèn)定,而且在相應(yīng)控制輸入作用下,跟蹤軌跡具有最優(yōu)意義,同時(shí)狀態(tài)觀測(cè)器也進(jìn)一步提高了反饋精度,因此在滿足輸入約束的前提下,系統(tǒng)能夠在較短時(shí)間段內(nèi)跟蹤指定軌跡,取得良好的跟蹤性能,表現(xiàn)為跟蹤路徑平滑、跟蹤快、速度平穩(wěn)。從圖4可以看出,如果在設(shè)計(jì)控制器時(shí)不考慮實(shí)際存在的控制約束和終端鎮(zhèn)定,閉環(huán)系統(tǒng)就很有可能會(huì)失去了穩(wěn)定性保證,也就很難獲得令人滿意的跟蹤效果。此外,圖6避障仿真結(jié)果同時(shí)也驗(yàn)證了所設(shè)計(jì)的避障功能模塊的有效性。

圖5 使用所設(shè)計(jì)的終端控制器的情況下的仿真結(jié)果

圖6 AGV避障仿真結(jié)果

4 結(jié)論

以三輪AGV為研究對(duì)象,研究存在非完整約束和控制輸入約束的AGV軌跡跟蹤的非線性模型預(yù)測(cè)控制問題,設(shè)計(jì)了一種終端鎮(zhèn)定控制器,并結(jié)合AGV的運(yùn)動(dòng)模型,使用狀態(tài)觀測(cè)器對(duì)含噪聲的動(dòng)態(tài)系統(tǒng)進(jìn)行狀態(tài)估計(jì),進(jìn)一步增強(qiáng)軌跡跟蹤控制的效果,實(shí)現(xiàn)了系統(tǒng)全局漸進(jìn)穩(wěn)定。此外還考慮AGV的避障功能,設(shè)計(jì)了一種避障控制算法。計(jì)算機(jī)仿真結(jié)果證實(shí)了所設(shè)計(jì)軌跡跟蹤算法的有效性和實(shí)時(shí)性。

[1] 閆茂德,吳青云,賀昱曜.非完整移動(dòng)機(jī)器人的自適應(yīng)滑模軌跡跟蹤控制[J].系統(tǒng)仿真學(xué)報(bào),2007,19(3):579-581.

[2] Dierks T,Jagannathan S.Control of Nonholonomic Mobile Robot Formations:Backstepping K inematics into Dynam ics[C]//16th IEEE International Conference on Control Applications,Part of IEEE Multi-conference on Systems and Control.Singapore,2007:94-99.

[3] Farzad P,Mattias PK.Adaptive Controlof Dynamic Mobile Robots with Nonholonom ic Constraints[J].Computers and Electrical Engineering,2002,28:241-253.

[4] Lin Sheng,Go ldenberg A A.Neural-netw ork Controlof Mobile Manipulators[J].IEEE Transactions on Neural Networks Manipulators,2001,12(5):1121-1133.

[5] Samson C.Controlof Chained Systems:App lication to Path Following and Time-varying Point-stabilization of Mobile Robots[J].IEEE Transactions on Automatic Contro l,1995,40:64-77.

[6] Fontes F A C C,Magni L.M in-max Model Predictive Controlof Nonlinear Systems Using Discontinuous Feedbacks[J].IEEE Transactions on Control System Techno logy,2003,48(10):1750-1755.

[7] Chen H,Allgow er F A.Quasi-infinite Horizon Non linear Modelp redictive Control Scheme with Guaranteed Stability[J].Automatica,1998,14(10):1205-1217.

[8] Mayne D,Raw ling J,Rao C,et al.Const Rained Model Predictive Control:Stability and Optimality[J].Automatica,2000,36:789-814.

[9] Timm F,Rolf F.Nonlinear Model Predictive Pathfollowing Control[J].Lecture Notes in Control and In formation Sciences,2009,384:335-343.

Non linear Model Predictive Control of Trajectory Tracking for Nonholonomic AGV

Zhao Han Yin Xiaohong Wu Yanming
Hefei University of Techno logy,H efei,230009

Considering the kinem atic model o f three-wheeled AGV w ith non-holonomic constraint and input saturation,a nonlinearm odel predictive controlwas studied and an united control algorithm of tracking and stability was proposed.Firstly,an optimal controllerwas obtained based on the princip les of model predictive control.Second ly,a terminal controller and an invariantterm inal region were designed in order to keep the AGV system stable.Then,the noise problem of state estim ation w as solved by using a state observer,which im proves the localization precision and enhances the control effectiveness.Finally,a module was designed to avoid obstacle.The designed control algorithm has generality for kinematic model of other wheeled mobile robots.Computer simulation results show its correctness and validness.

automatic guided vehicle(AGV);model predictive control;stability;observer;trajectory tracking

TP24

1004—132X(2011)06—0681—06

2010—05—24

國家“十一五”支撐計(jì)劃資助項(xiàng)目(2006036003131);國家高技術(shù)研究發(fā)展計(jì)劃(863計(jì)劃)資助項(xiàng)目(2006AA 11A 109)

(編輯 蘇衛(wèi)國)

趙 韓,男,1957年生。合肥工業(yè)大學(xué)機(jī)械與汽車工程學(xué)院教授、博士研究生導(dǎo)師。主要研究方向?yàn)閿?shù)字化設(shè)計(jì)與制造、汽車、智能車、磁力機(jī)械等。發(fā)表論文300余篇。尹曉紅,女,1985年生。合肥工業(yè)大學(xué)機(jī)械與汽車工程學(xué)院博士研究生。吳焱明,男,1968年生。合肥工業(yè)大學(xué)機(jī)械與汽車工程學(xué)院副教授。