葉伯生，譚帥，黎晗，潘釗，唐永杰

(華中科技大學(xué)機(jī)械科學(xué)與工程學(xué)院，湖北武漢 430074)

0 前言

移動(dòng)物流機(jī)器人(AGV)憑借快速、高效等特點(diǎn)正逐漸取代人工，引導(dǎo)傳統(tǒng)物流向智能物流轉(zhuǎn)變。受新冠肺炎疫情影響，少接觸和無接觸的辦公模式更是加快了AGV的部署速度。兩輪差速AGV因低成本優(yōu)勢(shì)，在倉儲(chǔ)物流行業(yè)得到了廣泛的應(yīng)用。但兩輪移動(dòng)機(jī)器人非完整性和強(qiáng)耦合性的特點(diǎn)，使其運(yùn)動(dòng)控制難度增加，這也使得其運(yùn)動(dòng)規(guī)劃與控制算法成為移動(dòng)機(jī)器人領(lǐng)域的研究熱門。在運(yùn)動(dòng)規(guī)劃方面，為了降低機(jī)器人的控制難度，規(guī)劃簡(jiǎn)單的直線圓弧組合軌跡是常見的做法，但該方法存在減速轉(zhuǎn)向問題，效率較低，因此現(xiàn)有研究多聚焦于規(guī)劃曲線軌跡以提高機(jī)器人效率。常用的曲線包括貝塞爾曲線、正弦曲線和多項(xiàng)式曲線。貝塞爾曲線控制靈活，保證軌跡平滑的同時(shí)能根據(jù)環(huán)境的動(dòng)態(tài)特性實(shí)時(shí)修改初始軌跡；但現(xiàn)有規(guī)劃方式普遍存在速度過度依賴軌跡形狀，難以參數(shù)化修改的問題。正弦曲線和多項(xiàng)式曲線過渡節(jié)點(diǎn)間處理困難，啟停過程中易發(fā)生加速度突變。在控制算法方面，常見的有模糊PID控制、Backstepping控制、自適應(yīng)神經(jīng)網(wǎng)絡(luò)控制、滑模控制、模型預(yù)測(cè)控制。模糊PID控制、Backstepping控制和自適應(yīng)神經(jīng)網(wǎng)絡(luò)控制在控制穩(wěn)定后均能獲得較好的控制效果，但在實(shí)驗(yàn)中常用于以勻速跟蹤圓或正弦軌跡，啟停過程存在速度突變，位姿誤差易受速度影響?；？刂瞥４嬖诙秳?dòng)，采用補(bǔ)償方式可有效削弱抖動(dòng)，但可能出現(xiàn)速度階躍，引入較大誤差。模型預(yù)測(cè)控制每次迭代都能獲得最優(yōu)解，可以獲得較優(yōu)的軌跡跟蹤性能，但運(yùn)算量大且可能出現(xiàn)無解，因此控制系統(tǒng)的實(shí)時(shí)性和可靠性較難保證。

綜上，本文作者一方面利用三次貝塞爾曲線擬合光滑軌跡，并通過跟蹤微分器離散化軌跡，獲得最大速度和最大加速度約束下的平滑速度曲線，解決了移動(dòng)機(jī)器人軌跡規(guī)劃和速度不連續(xù)問題；另一方面，在建立機(jī)器人位姿誤差模型的基礎(chǔ)上研究了機(jī)器人的Backstepping跟蹤控制方法，解決了機(jī)器人軌跡跟蹤控制問題；最后，將軌跡規(guī)劃算法與控制算法結(jié)合，通過規(guī)劃速度平滑和可參數(shù)化調(diào)整的軌跡，降低軌跡對(duì)控制器性能的影響，保證控制器平穩(wěn)地跟蹤曲線，提高控制精度。

1 軌跡規(guī)劃

1.1 參考軌跡與參考速度規(guī)劃

AGV在執(zhí)行貨物搬運(yùn)任務(wù)前，調(diào)度系統(tǒng)將可用路徑點(diǎn)序列的坐標(biāo)信息下發(fā)給控制系統(tǒng)，機(jī)器人根據(jù)接收到的路徑點(diǎn)序列進(jìn)行軌跡規(guī)劃。圖1表示AGV物流機(jī)器人將貨物從點(diǎn)搬運(yùn)到點(diǎn)所規(guī)劃的直線圓弧軌跡和曲線軌跡。為提升AGV貨物搬運(yùn)效率，采用基于貝塞爾曲線的機(jī)器人軌跡規(guī)劃方法。階貝塞爾曲線的表達(dá)形式為

圖1 AGV貨物搬運(yùn)過程

(1)

(2)

為滿足起點(diǎn)和終點(diǎn)姿態(tài)的獨(dú)立約束條件，至少需要三階貝塞爾曲線生成軌跡。更高階的貝塞爾曲線雖然提升了軌跡的平滑性與靈活性，但硬件的運(yùn)算負(fù)擔(dān)也隨之加劇，不利于實(shí)時(shí)控制。綜上，以生成軌跡所需的最低階貝塞爾曲線為研究對(duì)象，采用三階貝塞爾曲線規(guī)劃機(jī)器人平面運(yùn)動(dòng)軌跡。由式(1)(2)得到參考軌跡的計(jì)算如下：

(3)

atan2(1-0,1-0)、atan2(3-2,3-2)分別表示機(jī)器人起點(diǎn)和終點(diǎn)的姿態(tài)。

求解曲線長(zhǎng)度，以規(guī)劃機(jī)器人速度。平面中貝塞爾曲線的總長(zhǎng)度可以運(yùn)用積分方法進(jìn)行計(jì)算：

(4)

其中：′()、′()為式(3)對(duì)求導(dǎo)的結(jié)果。式(4)的積分表達(dá)式通常難以獲得，因此采用數(shù)值法求取曲線的長(zhǎng)度。將貝塞爾曲線的控制參數(shù)均勻細(xì)分為份，應(yīng)用Simpson公式求解曲線長(zhǎng)度：

(5)

(6)

AGV的緩慢啟停和整個(gè)運(yùn)行過程的速度平滑是保證貨物安全運(yùn)送的關(guān)鍵，讓機(jī)器人在最大速度和最大加速度允許范圍內(nèi)進(jìn)行工作是機(jī)器人穩(wěn)定運(yùn)行的前提。針對(duì)上述要求，本文作者提出將自抗擾中獲得巨大成功的跟蹤微分器進(jìn)行改造，將其運(yùn)用到軌跡、速度和加速度受約束條件下的機(jī)器人軌跡規(guī)劃中。該算法有兩種實(shí)現(xiàn)方案：若不考慮加加速度約束，則只需改變最大速度和最大加速度兩個(gè)參數(shù)就能參數(shù)化修改機(jī)器人速度曲線，實(shí)現(xiàn)降速和提速，但存在速度不連續(xù)問題；若考慮加加速度約束，則不僅能實(shí)現(xiàn)軌跡的參數(shù)化修改，還能保證整個(gè)速度曲線的平滑連續(xù)，因此該方案是本文作者的研究重點(diǎn)。上述兩種方法通過跟蹤微分器對(duì)長(zhǎng)度為的貝塞爾曲線進(jìn)行速度規(guī)劃的過程分為跟蹤微分和積分兩個(gè)過程。兩種方案參考速度的計(jì)算方法如下：

算法 1：不考慮加加速度約束

(7)

算法2：考慮加加速度約束

(8)

其中:fhan的函數(shù)表達(dá)式如下：

fhan(,,,)=

(9)

式(7)(8)為兩種算法積分過程的數(shù)學(xué)描述；式(9)為跟蹤微分過程的數(shù)學(xué)描述。其中：、、分別表示最大速度、最大加速度和最大加加速度；表示參考加加速度；代表控制周期；代表第個(gè)離散點(diǎn)；()、()、()分別代表·時(shí)刻所對(duì)應(yīng)的軌跡長(zhǎng)度、參考速度和參考加速度；sign為符號(hào)函數(shù)。

1.2 參考位姿與參考角速度規(guī)劃

經(jīng)過第1.1節(jié)的推導(dǎo)已經(jīng)得到了參考速度()，為了完成軌跡規(guī)劃，還需要規(guī)劃機(jī)器人參考角速度()和參考位姿(()，()，())。為此，需獲得軌跡長(zhǎng)度()與貝塞爾曲線控制參數(shù)()的映射關(guān)系。為便于實(shí)時(shí)控制，采用Newton插值法快速獲得()對(duì)應(yīng)的()，再通過()離散化軌跡，獲得對(duì)應(yīng)的位姿。因軌跡長(zhǎng)度關(guān)于控制參數(shù)單調(diào)遞增，因而對(duì)于任意()，有且僅有一個(gè)()與之對(duì)應(yīng)。參考角速度與參考位姿的計(jì)算過程如下：

步驟1,由式(10)初始化參數(shù)()；

步驟2,由式(5)計(jì)算(())；

步驟3,當(dāng)′()≠0時(shí)，已知曲線上任意一點(diǎn)對(duì)應(yīng)的軌跡長(zhǎng)度()，可由式(11)的Newton迭代公式更新參數(shù)()；

步驟4,由式(11)對(duì)()進(jìn)行迭代，得到()對(duì)應(yīng)的精確解()；

步驟5,將()代入式(3)中的貝塞爾曲線方程，實(shí)時(shí)更新參考位置(()，())；

步驟6,由式(12)，計(jì)算參考角度和參考角速度。

(10)

(11)

()=atan2((),())

(12)

2 機(jī)器人運(yùn)動(dòng)學(xué)模型

文中的研究對(duì)象由兩個(gè)萬向輪和驅(qū)動(dòng)輪分別提供支撐和驅(qū)動(dòng)，萬向輪對(duì)機(jī)器人運(yùn)動(dòng)的影響可以忽略，因此機(jī)器人的運(yùn)動(dòng)學(xué)模型可簡(jiǎn)化為圖2所示的兩輪差速模型。模型中：-為參考坐標(biāo)系(世界坐標(biāo)系)；-為機(jī)器人坐標(biāo)系；點(diǎn)(點(diǎn))表示AGV的質(zhì)心；為兩輪之間的距離，l、r分別表示機(jī)器人的左、右輪；、分別代表機(jī)器人參考速度和參考角度；、、分別表示機(jī)器人質(zhì)心實(shí)際速度、角速度和角度，它們相對(duì)于參考系-描述；、表示位置誤差，表示姿態(tài)角誤差，它們都相對(duì)于坐標(biāo)系-描述。

圖2 機(jī)器人簡(jiǎn)化運(yùn)動(dòng)學(xué)模型

運(yùn)動(dòng)過程中，機(jī)器人兩輪速度和質(zhì)心速度滿足如下約束關(guān)系：

(13)

其中:為車輪半徑。由圖2中的機(jī)器人運(yùn)動(dòng)學(xué)模型建立誤差間的坐標(biāo)變換關(guān)系，得到機(jī)器人位姿誤差方程：

(14)

其中:、表示機(jī)器人質(zhì)心位置。將上式對(duì)時(shí)間求導(dǎo)，并由防滑約束關(guān)系得到位姿誤差的微分形式：

(15)

3 控制器設(shè)計(jì)

基于Backstepping方法，設(shè)計(jì)AGV軌跡跟蹤控制器：

步驟1，由位置誤差定義如下Lyapunov函數(shù)：

(16)

≥0，僅當(dāng)、項(xiàng)均為0時(shí)等號(hào)成立。將各項(xiàng)對(duì)時(shí)間求偏導(dǎo)，聯(lián)立式(15)得到如下關(guān)系式：

(17)

根據(jù)Lyapunov穩(wěn)定性判據(jù)，為滿足項(xiàng)的穩(wěn)定性條件，線速度的控制率取值如下：

=+cos

(18)

式中：>0。為滿足項(xiàng)的穩(wěn)定性條件，建立變量間的虛擬約束關(guān)系，引入如下虛擬控制量：

(19)

令=，考察項(xiàng)的穩(wěn)定性變?yōu)榕袛鄐in()≤0是否成立。求導(dǎo)知該不等式恒成立，且僅當(dāng)=0時(shí)等號(hào)成立。聯(lián)立式(17)(18)(19)得：

(20)

步驟2，為實(shí)現(xiàn)姿態(tài)角跟蹤，定義如下誤差變量：

=-

(21)

結(jié)合Backstepping思想，定義如下Lyapunov函數(shù)：

(22)

≥0，僅當(dāng)、均為0時(shí)等號(hào)成立。進(jìn)一步對(duì)各項(xiàng)進(jìn)行求導(dǎo)，得到如下關(guān)系：

(23)

(24)

(25)

(26)

(27)

整理上面的表達(dá)式，最后得到如下關(guān)系：

(28)

由上式知的跟蹤性能只與相關(guān)，的跟蹤性能只與相關(guān)，是個(gè)二階系統(tǒng)，其跟蹤性能與頻率和阻尼相關(guān)。將式(28)化簡(jiǎn)為二階微分方程的一般形式：

(29)

4 仿真及實(shí)驗(yàn)驗(yàn)證

搭建如圖3所示的AGV實(shí)物平臺(tái)和仿真平臺(tái)，驗(yàn)證軌跡規(guī)劃算法和控制算法的正確性。為保證實(shí)物平臺(tái)與仿真平臺(tái)軌跡跟蹤效果的一致性，將仿真平臺(tái)中機(jī)器人的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)參數(shù)設(shè)置成與AGV實(shí)物一致。機(jī)器人的長(zhǎng)、寬、高分別為1 000、 750、300 mm；輪子半徑=90 mm，兩輪中心的橫向距離=711 mm。

圖3 實(shí)驗(yàn)平臺(tái)

4.1 仿真

4.1.1 大誤差下軌跡跟蹤仿真

為了測(cè)試算法的可靠性，實(shí)驗(yàn)中人為地將機(jī)器人參考軌跡起點(diǎn)和實(shí)際軌跡的起點(diǎn)設(shè)置在相差較大的位置。仿真在V-REP軟件中進(jìn)行，由Newton物理引擎計(jì)算，坐標(biāo)均以mm為單位計(jì)量。仿真時(shí)用于軌跡規(guī)劃的4個(gè)控制點(diǎn)的坐標(biāo)分別為：(0,0)、(2 100,3 200)、(4 900,800)、(7 000,4 000)?？紤]不同初始狀態(tài)下，機(jī)器人軌跡收斂情況不一致，為此設(shè)計(jì)了機(jī)器人在4種不同初始狀態(tài)下的仿真實(shí)驗(yàn)。4個(gè)實(shí)驗(yàn)中，機(jī)器人起點(diǎn)分別位于世界坐標(biāo)系的4個(gè)象限中，且每次相對(duì)于坐標(biāo)原點(diǎn)的偏移量都是機(jī)器人機(jī)身的長(zhǎng)度和寬度。4次實(shí)驗(yàn)的起點(diǎn)坐標(biāo)分別為(-750,-1 000)、(750,1 000)、(-750,1 000)、(750,-1 000)，姿態(tài)角均為0 rad。仿真時(shí)采用第1.1節(jié)中的算法2進(jìn)行軌跡規(guī)劃，跟蹤微分器的相關(guān)參數(shù)分別設(shè)置為=0.05 s、=600 mm/s、=500 mm/s、=1 200 mm/s?？刂破鞯南嚓P(guān)參數(shù)分別取為=8.2、=0.5。實(shí)驗(yàn)結(jié)果如圖4所示。

圖4 軌跡規(guī)劃及軌跡跟蹤仿真實(shí)驗(yàn)效果

由圖4(a)可知：即使起點(diǎn)存在較大的位姿誤差，在控制作用下機(jī)器人均能逐漸向規(guī)劃的軌跡收斂，并完成軌跡跟蹤。觀察圖4(b)的參考速度可知：規(guī)劃的速度曲線能滿足速度和加速度的約束，說明了跟蹤微分器進(jìn)行軌跡規(guī)劃的可行性。觀察圖4(b)中的速度跟蹤曲線可知:由于起點(diǎn)誤差的原因，在加速階段為了減小跟蹤誤差，機(jī)器人實(shí)時(shí)對(duì)自身速度進(jìn)行調(diào)整，出現(xiàn)實(shí)際速度偏離規(guī)劃速度的現(xiàn)象；到達(dá)勻速階段后，誤差不斷減小，機(jī)器人實(shí)際速度與規(guī)劃速度逐漸貼合，并最終完成速度跟蹤。仿真結(jié)果驗(yàn)證了軌跡規(guī)劃方法的可行性和控制器的可靠性。

4.1.2 小誤差下軌跡跟蹤仿真

機(jī)器人實(shí)際運(yùn)行速度曲線能證明軌跡規(guī)劃的有效性，誤差曲線能反映控制器的跟蹤控制性能。為此，設(shè)計(jì)了兩組對(duì)照實(shí)驗(yàn)。第一組對(duì)照實(shí)驗(yàn)是機(jī)器人未規(guī)劃速度曲線與用跟蹤微分器進(jìn)行速度規(guī)劃的軌跡跟蹤對(duì)比實(shí)驗(yàn)；第二組對(duì)照實(shí)驗(yàn)是采用滑?？刂破髋c文中設(shè)計(jì)的控制器分別對(duì)機(jī)器人進(jìn)行軌跡跟蹤控制的控制效果對(duì)比實(shí)驗(yàn)。仿真實(shí)驗(yàn)中，用于控制生成貝塞爾曲線的4個(gè)控制點(diǎn)的坐標(biāo)分別為：(0,0)、(646,2 290)、(3 878,4 122)、(6 463,4 580)；跟蹤微分器與控制器的參數(shù)設(shè)置與第4.1.1節(jié)中一致；未規(guī)劃速度的軌跡跟蹤實(shí)驗(yàn)中，參照文獻(xiàn)[17]不單獨(dú)對(duì)速度進(jìn)行規(guī)劃，將機(jī)器人參考速度設(shè)置為1 200 mm/s；滑?？刂破鞯慕Y(jié)構(gòu)形式參照文獻(xiàn)[12]中的VSC控制器進(jìn)行設(shè)計(jì)。實(shí)驗(yàn)中，機(jī)器人初始位置誤差均為0，初始姿態(tài)角誤差均在0.035 rad以內(nèi)。仿真結(jié)果如圖5所示。

分析圖5中各組實(shí)驗(yàn)的速度曲線和位姿誤差曲線，未進(jìn)行速度規(guī)劃時(shí)，機(jī)器人在啟停階段存在速度階躍，導(dǎo)致產(chǎn)生較大的位姿誤差；采用滑?？刂破鬟M(jìn)行控制時(shí)，機(jī)器人在向規(guī)劃軌跡收斂的過程中容易產(chǎn)生速度波動(dòng)，導(dǎo)致出現(xiàn)嚴(yán)重的顫振現(xiàn)象；采用文中的方法進(jìn)行控制時(shí)，機(jī)器人實(shí)現(xiàn)了以平滑連續(xù)的速度在接近零的誤差范圍內(nèi)跟蹤期望軌跡。該結(jié)果表明：文中的軌跡規(guī)劃方法與控制器設(shè)計(jì)的有效性和可靠性。

圖5 實(shí)驗(yàn)對(duì)比

4.2 實(shí)驗(yàn)

實(shí)驗(yàn)主要是為了驗(yàn)證控制器的有效性與仿真結(jié)果的正確性。實(shí)驗(yàn)在由1 000 mm×1 000 mm正方形網(wǎng)格組成的鋁板上進(jìn)行，機(jī)器人實(shí)時(shí)位姿由里程計(jì)和慣導(dǎo)經(jīng)融合后解算獲取。實(shí)驗(yàn)過程中，為了同時(shí)驗(yàn)證控制器跟蹤存在速度不連續(xù)軌跡的有效性，采用不考慮加加速度約束的軌跡規(guī)劃方案，該方案規(guī)劃的軌跡在加減速階段仍存在加速度突變。實(shí)驗(yàn)中貝塞爾曲線的4個(gè)控制點(diǎn)坐標(biāo)分別為：(1 996,7 016)、(2 046,7 758)、(2 298,8 352)、(2 500,8 500)?？刂破鞯膮?shù)與第4.1.1節(jié)中設(shè)置的一致。跟蹤微分器的參數(shù)分別設(shè)置為=0.02 s、=600 mm/s、=1 000 mm/s。實(shí)驗(yàn)開始前，機(jī)器人質(zhì)心在地面上的投影點(diǎn)位于處，車頭指向的切線方向。實(shí)驗(yàn)結(jié)果如圖6所示。

觀察圖6中的位姿誤差曲線，整個(gè)軌跡跟蹤過程中方向誤差在±20 mm區(qū)間內(nèi)波動(dòng)，方向誤差在±10 mm區(qū)間內(nèi)波動(dòng)，角度誤差接近0。分析圖6中的實(shí)驗(yàn)結(jié)果，整個(gè)軌跡跟蹤過程中，機(jī)器人均能穩(wěn)定跟蹤規(guī)劃的軌跡，即使在存在速度突變的加減速階段，機(jī)器人仍能夠較好地跟蹤參考速度和參考軌跡，這說明了控制器的可靠性，證明了文中所設(shè)計(jì)的控制算法的有效性。

圖6 軌跡規(guī)劃及軌跡跟蹤效果

方向出現(xiàn)較大的誤差可能是由于速度不連續(xù)導(dǎo)致加速度突變，產(chǎn)生了打滑。實(shí)際上，在推導(dǎo)機(jī)器人位姿態(tài)誤差方程時(shí)，采用了防滑約束關(guān)系，因此若出現(xiàn)打滑將可能影響跟蹤效果。分析方向的位姿誤差曲線可知，機(jī)器人最大位置誤差實(shí)際上是出現(xiàn)在機(jī)器人速度不連續(xù)的階段，而這些階段恰是最容易出現(xiàn)打滑的階段。事實(shí)上，該實(shí)驗(yàn)進(jìn)一步驗(yàn)證了引言中所討論的問題，即軌跡規(guī)劃不考慮速度連續(xù)問題會(huì)因?yàn)榧铀俣韧蛔儗?dǎo)致跟蹤誤差倍增。

5 結(jié)論

同時(shí)研究了機(jī)器人的軌跡規(guī)劃和跟蹤控制問題。首先，提出利用貝塞爾曲線解決直線圓弧軌跡存在的劇烈轉(zhuǎn)向問題；接著，提出了保證AGV安全穩(wěn)定運(yùn)行的約束條件，并將約束條件作為跟蹤微分器的輸入獲得了平滑的參考速度曲線；然后，基于Backstepping方法進(jìn)行了軌跡跟蹤控制器的設(shè)計(jì)，并分析了控制器參數(shù)對(duì)跟蹤性能的影響；最后，搭建了V-REP仿真實(shí)驗(yàn)環(huán)境和AGV實(shí)物實(shí)驗(yàn)環(huán)境對(duì)算法進(jìn)行驗(yàn)證。相比于未進(jìn)行速度規(guī)劃、滑?？刂坪退俣炔贿B續(xù)等控制方式，文中提出的軌跡規(guī)劃方法與控制算法在軌跡跟蹤過程中，降低了軌跡對(duì)控制器性能的影響，提升了控制穩(wěn)定性和跟蹤精度。