基于Lyapunov第二方法的自動(dòng)導(dǎo)引車軌跡跟蹤控制器設(shè)計(jì)與仿真

楊勇生 趙宏 姚海慶

摘要：為實(shí)現(xiàn)港口運(yùn)載設(shè)備的自動(dòng)化，研究港口用自動(dòng)導(dǎo)引車（automated guided vehicle， AGV）的軌跡跟蹤控制問題。構(gòu)建前輪轉(zhuǎn)向、后輪驅(qū)動(dòng)的AGV運(yùn)動(dòng)學(xué)模型，運(yùn)用Lyapunov第二方法保證系統(tǒng)穩(wěn)定。設(shè)計(jì)一種受非完整約束的AGV軌跡跟蹤控制器。采用真實(shí)的AGV樣車參數(shù)，對(duì)直線和圓弧軌跡跟蹤進(jìn)行仿真。仿真結(jié)果表明該控制器具有收斂速度快、跟蹤誤差小等特性。

關(guān)鍵詞：港口; 自動(dòng)導(dǎo)引車（AGV）; 軌跡跟蹤; Lyapunov第二方法

中圖分類號(hào)：U691.7;TP242

文獻(xiàn)標(biāo)志碼：A

Abstract：In order to realize the automation of carrier vehicles for ports， the trajectory tracking control of automated guided vehicles （AGVs） for ports is studied. The AGV kinematics model of front-wheel steering and rear-wheel drive is constructed， and the system stability is guaranteed by Lyapunovs second method. A trajectory tracking controller for AGVs with nonholonomic constraints is designed. Tracking trajectories composed of straight lines and arcs are simulated using parameters of AGV solid prototype. The simulation results show that the controller has the characteristics of fast convergence and low tracking error.

Key words：port; automated guided vehicle （AGV）; trajectory tracking; Lyapunovs second method

收稿日期：2018-03-26

修回日期：2018-06-20

基金項(xiàng)目：國(guó)家自然科學(xué)基金（61540045）;上海市科學(xué)技術(shù)委員會(huì)重點(diǎn)項(xiàng)目（14170501500）;上海海事大學(xué)研究生創(chuàng)新基金（2017ycx052）

作者簡(jiǎn)介：

楊勇生（1965—），男，江西南昌人，教授，博導(dǎo)，博士，研究方向?yàn)樽詣?dòng)化碼頭協(xié)同作業(yè)調(diào)度與控制、港口物流裝備及其智能化，（E-mail）ysyang@shmtu.edu.cn

0 引 言

隨著區(qū)域經(jīng)濟(jì)一體化進(jìn)程和經(jīng)濟(jì)全球化趨勢(shì)的加快，國(guó)際物流運(yùn)輸行業(yè)飛速發(fā)展，航運(yùn)物流在進(jìn)入集裝箱化時(shí)代后又向多式聯(lián)運(yùn)和現(xiàn)代物流方向發(fā)展[1]。集裝箱碼頭吞吐量增長(zhǎng)迅速，碼頭作業(yè)強(qiáng)度不斷增加，急速推動(dòng)集裝箱碼頭向自動(dòng)化、智能化轉(zhuǎn)型，

作業(yè)效率不斷提高，人力成本不斷降低。港口作業(yè)區(qū)域可以分為岸邊作業(yè)區(qū)、水平運(yùn)輸區(qū)和堆場(chǎng)作業(yè)區(qū)，其中具有中轉(zhuǎn)作用的水平運(yùn)輸區(qū)的運(yùn)輸設(shè)備由傳統(tǒng)碼頭的集卡轉(zhuǎn)化為自動(dòng)導(dǎo)引車（automated guided vehicle，AGV）、地面自主車（autonomous land vehicle，ALV）和自動(dòng)跨運(yùn)車。從全球看，AGV是水平運(yùn)輸設(shè)備中使用率最高的，其優(yōu)勢(shì)體現(xiàn)在高自動(dòng)化、高智能化和環(huán)保性上。AGV的作業(yè)過程是接收遠(yuǎn)程的控制指令，根據(jù)預(yù)定路線完成貨物運(yùn)輸?shù)倪^程，其軌跡跟蹤技術(shù)作為AGV的關(guān)鍵技術(shù)，一直是學(xué)者們研究的熱點(diǎn)。

謝德勝等[2]提出了一種基于RTK-GPS（real-time kinematic GPS）的軌跡跟蹤方法，將軌跡地圖和移動(dòng)機(jī)器人的經(jīng)緯度值轉(zhuǎn)換到平面坐標(biāo)系，運(yùn)用標(biāo)準(zhǔn)卡爾曼濾波對(duì)機(jī)器人的定位數(shù)據(jù)進(jìn)行濾波，采用PID控制器控制機(jī)器人的行進(jìn)和轉(zhuǎn)向;AL-KHATIB等[3]使用擴(kuò)展卡爾曼算法，將編碼器、指南針、GPS、陀螺儀和加速度傳感器進(jìn)行了多種組合和全部融合試驗(yàn)，通過對(duì)比試驗(yàn)效果證明全部融合的定位信息更加準(zhǔn)確;張慶等[4]提出了基于多傳感器融合的運(yùn)動(dòng)目標(biāo)跟蹤算法，采用混合高斯算法對(duì)運(yùn)動(dòng)目標(biāo)的背景進(jìn)行建模，使用均值漂移算法實(shí)現(xiàn)運(yùn)動(dòng)目標(biāo)追蹤;YANG等[5]提出了一種以四輪驅(qū)動(dòng)車為模型、考慮車輛速度響應(yīng)的軌跡跟蹤控制方案，采用模型預(yù)測(cè)控制方法構(gòu)建的動(dòng)態(tài)軌跡規(guī)劃模型容易滿足各種物理約束，解決了處理器計(jì)算能力受限的問題，提高了閉環(huán)跟蹤系統(tǒng)的魯棒性;邢艷榮等[6]提出了一種基于自適應(yīng)滑?？刂频乃妮喨蛞苿?dòng)機(jī)器人軌跡控制方法，利用徑向基函數(shù)（radial basis function， RBF）神經(jīng)網(wǎng)絡(luò)實(shí)時(shí)調(diào)整軌跡跟蹤控制器的切換增益，以減少系統(tǒng)的抖振，通過仿真驗(yàn)證了其抗干擾能力;李琳等[7]提出了一種可以彌補(bǔ)傳統(tǒng)遺傳算法不足的免疫遺傳算法，通過對(duì)直流伺服電機(jī)系統(tǒng)進(jìn)行控制器參數(shù)整定，在遺傳算法中加入免疫機(jī)制，引入抗體濃度概念，進(jìn)一步提高了算法的搜索能力;許亞芳等[8-10]利用激光雷達(dá)、攝像頭等傳感器獲取周邊位置信息，利用多次測(cè)量更新算法進(jìn)行即時(shí)定位與地圖構(gòu)建（simultaneous localization and mapping， SLAM），抑制了誤差在機(jī)器人位姿和特征點(diǎn)位置中的傳播，降低了整個(gè)系統(tǒng)的狀態(tài)誤差;CUI等[11]提出了一種針對(duì)縱向和橫向滑動(dòng)的履帶式機(jī)器人軌跡控制器，使用帶有低通的無損卡爾曼濾波器對(duì)滑動(dòng)誤差進(jìn)行濾波;SENADHEERA等[12]僅使用反應(yīng)轉(zhuǎn)矩觀測(cè)器獲得不同地形的滾動(dòng)阻力矩來計(jì)算滑移率和其他表面觀測(cè)值，以此進(jìn)行地形估計(jì);MAZULINA等[13]使用超聲波傳感器、陀螺儀和計(jì)算機(jī)視覺對(duì)運(yùn)動(dòng)表面進(jìn)行評(píng)估后決定是否繞行該區(qū)域;周波等[14]利用激光雷達(dá)對(duì)機(jī)器人進(jìn)行3D室外模型實(shí)時(shí)建模，直接給出了高斯描述的概率化地形估計(jì)，在保證實(shí)時(shí)性的前提下，提高了地形模型的性能。

YIN等[15]提出一種融合神經(jīng)動(dòng)力學(xué)、節(jié)能追蹤、滑模自適應(yīng)控制的路徑分段控制方法，但該方法對(duì)于參數(shù)選擇很敏感;ROJAS-CONTRERAS等[16]對(duì)使用不同的輸入控制參數(shù)的兩種滑?？刂破鬟M(jìn)行了對(duì)比試驗(yàn)，指出兩者均能減少外界因素干擾造成的誤差，但缺點(diǎn)是對(duì)于方向改變的動(dòng)態(tài)響應(yīng)較弱;ZDEAR等[17]建立了基于視覺軌跡追蹤模型的移動(dòng)機(jī)器人控制模型，通過高架攝像機(jī)獲取能定位機(jī)器人的圖像信息，但由于攝像機(jī)放在較高的位置，若其行進(jìn)速度較快則會(huì)造成較大的晃動(dòng)，丟失定位目標(biāo)。

以上文獻(xiàn)對(duì)解決車體的定位和控制問題提供了有價(jià)值的參考，但是考慮到軌跡控制方法的有效性、控制器的可實(shí)現(xiàn)性，以及Lyapunov第二方法不要求精確的數(shù)學(xué)模型的特性[18]，針對(duì)港口集裝箱搬運(yùn)問題，建立前輪轉(zhuǎn)向、后輪驅(qū)動(dòng)的AGV運(yùn)動(dòng)學(xué)模型，在此基礎(chǔ)上用Lyapunov第二方法使系統(tǒng)穩(wěn)定，構(gòu)造滿足條件的AGV控制律，并通過仿真證明該控制器的有效性和可行性。

1 AGV運(yùn)動(dòng)學(xué)模型

本文以課題組現(xiàn)有的AGV樣車（見圖1）為研究對(duì)象，參照其車體參數(shù)，構(gòu)建運(yùn)動(dòng)學(xué)模型。

研究對(duì)象是如圖2所示的阿克曼轉(zhuǎn)向結(jié)構(gòu)[19]，其中前輪為轉(zhuǎn)向輪，后輪為驅(qū)動(dòng)輪。圖2中L為軸距，δ為前輪轉(zhuǎn)向角，R為轉(zhuǎn)彎半徑，L=1.63 m，Rmin=3.5 m。由圖2可得R=L/tan δ，從而可得δmax=24.97°。

簡(jiǎn)化車體模型以AGV的兩個(gè)后輪中心點(diǎn)連線的中點(diǎn)C作為參考點(diǎn)，C點(diǎn)坐標(biāo)（XC，YC，θC）即為AGV的位置，其中θC為AGV的航向角。將AGV的運(yùn)動(dòng)軌跡離散化，設(shè)采樣周期為Δt，C點(diǎn)的坐標(biāo)在時(shí)刻 tk和tk+1分別為（Xk，Yk，θk）和（Xk+1，Yk+1，θk+1），建立如圖3所示的AGV運(yùn)動(dòng)學(xué)模型，其中vC為C點(diǎn)的線速度，ωC為航向角速度。根據(jù)運(yùn)動(dòng)學(xué)模型，離散化的控制參數(shù)運(yùn)算方程為2 基于Lyapunov第二方法的AGV軌跡跟蹤控制AGV的運(yùn)動(dòng)控制體現(xiàn)在控制車體的線速度和角速度上。當(dāng)不考慮車輪的滑動(dòng)時(shí)，在AGV工作平面內(nèi)建立直角坐標(biāo)系。設(shè)vC和θC分別為在時(shí)刻tk由定位傳感器獲取的線速度和姿態(tài)角，（XR，YR，θR）為利用運(yùn)動(dòng)學(xué)模型推算出的在坐標(biāo)系XOY上的期望位姿。

以兩個(gè)后輪中心點(diǎn)連線中點(diǎn)C為原點(diǎn)，以當(dāng)前的線速度方向?yàn)閤軸方向、x軸逆時(shí)針90°方向?yàn)閥軸方向建立局部坐標(biāo)系xCy，建立如圖4所示的軌跡誤差跟蹤模型。

為使跟蹤誤差E=（ex ey eθ）T能夠漸進(jìn)穩(wěn)定

根據(jù)Lyapunov意義下的漸進(jìn)穩(wěn)定的充分必要條件E·k<0，可以構(gòu)造如下控制方程：

對(duì)于任意輸入下的跟蹤誤差E，當(dāng)E≠0時(shí)，Ek>0，E·<0。綜上所述，基于式（4）所設(shè)計(jì)的軌跡跟蹤控制器（其結(jié)構(gòu)見圖5），滿足在連續(xù)運(yùn)行的條件下控制系統(tǒng)的誤差能夠收斂到0的要求。

由圖5可知，該軌跡跟蹤控制器由位姿反饋控制器（即定位傳感器，見圖中虛框2）和基于Lyapunov函數(shù)的自適應(yīng)反演控制的位姿補(bǔ)償器（虛框1）組成，形成一個(gè)閉環(huán)控制回路，實(shí)現(xiàn)對(duì)給定參考軌跡的跟蹤控制。位姿信息由北斗差分系統(tǒng)、慣性導(dǎo)航模塊和激光雷達(dá)等設(shè)備采集到的位置數(shù)據(jù)進(jìn)行融合得到。

3 軌跡跟蹤控制器的MATLAB仿真

通過以上設(shè)計(jì)構(gòu)建能夠跟蹤給定參考軌跡的控制器。為驗(yàn)證其有效性，利用MATLAB進(jìn)行軌跡跟蹤仿真。由于在港口運(yùn)輸中車輛路徑多由圓弧和直線組成，故選用直線軌跡和圓軌跡作為參考軌跡，采用課題組的AGV樣車參數(shù)對(duì)其控制過程進(jìn)行仿真。由于定位傳感器給AGV發(fā)送數(shù)據(jù)的最大頻率為10 Hz，仿真中也以這個(gè)頻率對(duì)位置進(jìn)行推算，且數(shù)值精確到小數(shù)點(diǎn)后第4位。

3.1 直線軌跡跟蹤仿真

參考軌跡是一條以（0，0）為起點(diǎn)、斜率為0.75的直線，其方程為Y=0.75X。考慮到樣車的可控速度范圍為0～6 m/s，滿載狀態(tài)下的最高速度為2 m/s，仿真時(shí)設(shè)置車輛速度恒為2 m/s。初始姿態(tài)誤差為ex=0.5 m，ey=0，eθ=1 rad，控制參數(shù)分別為k1=15，k2=1.2，k3=5.3。仿真結(jié)果見圖6。

仿真結(jié)果表明，控制器能夠完成對(duì)軌跡的追蹤，具有較快的響應(yīng)速度。誤差ex比其他兩個(gè)誤差收

斂快，能夠在5 s左右穩(wěn)定，ey和eθ也能夠在10 s內(nèi)穩(wěn)定。

3.2 圓軌跡跟蹤仿真

在實(shí)際作業(yè)中，港口碼頭路面建設(shè)標(biāo)準(zhǔn)寬度[21]為7.0～10.5 m，且AGV樣車的最小轉(zhuǎn)彎半徑Rmin為3.5 m，在考慮一定的轉(zhuǎn)向角冗余量的基礎(chǔ)上，選取9 m作為圓軌跡的半徑。

將速度和轉(zhuǎn)彎半徑作為仿真變量。初始姿態(tài)誤差為ex=-0.5 m，ey=0.5 m，eθ=0.1 rad，控制參數(shù)分別為k1=5，k2=47.8，k3=8.7。仿真結(jié)果見圖7。

從仿真結(jié)果可以得到，本文所建立的控制器能夠較快、平穩(wěn)地跟蹤直線軌跡和圓軌跡，且將誤差控制在10 mm內(nèi)。不同的控制器參數(shù)會(huì)影響其穩(wěn)定響應(yīng)時(shí)間和穩(wěn)態(tài)誤差，針對(duì)軌跡的變化，需要選用不同的控制器參數(shù)以達(dá)到較優(yōu)的控制效果。

4 結(jié) 論

本文對(duì)港口自動(dòng)導(dǎo)引車（AGV）的軌跡跟蹤控制進(jìn)行了研究，建立了AGV運(yùn)動(dòng)學(xué)模型，使用Lyapunov函數(shù)構(gòu)造了穩(wěn)定的軌跡跟蹤控制器，構(gòu)建了符合港口碼頭道路標(biāo)準(zhǔn)的參考軌跡，采用實(shí)體AGV樣車參數(shù)進(jìn)行仿真，達(dá)到了快速、平穩(wěn)的控制效果，驗(yàn)證了控制器的有效性，表明該控制器具有一定的工程實(shí)用前景。下一步將進(jìn)一步優(yōu)化控制器，使其能夠針對(duì)不同參考軌跡自行調(diào)整控制器參數(shù)，達(dá)到更好的控制性能。

參考文獻(xiàn)：

[1]樊躍進(jìn). 港口集裝箱AGV自動(dòng)搬運(yùn)技術(shù)可行性淺析[J]. 港口裝卸， 2006（3）：6-9.

[2]謝德勝， 徐友春， 萬劍， 等. 基于RTK-GPS的輪式移動(dòng)機(jī)器人軌跡跟隨控制[J]. 機(jī)器人， 2017， 39（2）：221-229. DOI：10.13973/j.cnki.robot.2017.0221.

[3]AL-KHATIB E I， JARADAT M A， ABDEL-HAFEZ M， et al. Multiple sensor fusion for mobile robot localization and navigation using the Extended Kalman Filter[C]//10th International Symposium on Mechatronics and its Applications， January 5， 2016， ISMA 2015. DOI：10.1109/ISMA.2015.7373480.

[4]張慶， 夏天， 范軼飛， 等. 基于多傳感器融合的運(yùn)動(dòng)目標(biāo)跟蹤算法[J]. 現(xiàn)代電子技術(shù)， 2017， 40（3）：43-46. DOI：10.16652/j.issn.1004-373x.2017.03.012.

[5]YANG Lu， YUE Ming， MA Teng， et al. Trajectory tracking control for 4WD vehicles using MPC and adaptive fuzzy control[C]// Proceedings of the 36th Chinese Control Conference （CCC）， Dalian， China， July 26-28， 2017：9367-9372.

[6]邢艷榮， 秦佳偉， 蘇彩云. 基于自適應(yīng)滑?？刂频乃妮喨蛞苿?dòng)機(jī)器人軌跡跟蹤方法研究[J]. 機(jī)械傳動(dòng)， 2015， 39（12）：39-43， 52. DOI：10.16578/j.issn.1004.2539.2015.12.008.

[7]李琳， 任俊霖， 鄒焱飚， 等. 基于免疫遺傳算法的移動(dòng)機(jī)器人軌跡跟蹤[J]. 華南理工大學(xué)學(xué)報(bào)（自然科學(xué)版）， 2013， 41（7）：13-18， 25. DOI：10.3969/ j.issn.1001-565X.2013.07.003.

[8]許亞芳， 孫作雷， 曾連蓀， 等. 基于多次測(cè)量更新的移動(dòng)機(jī)器人SLAM仿真[J]. 系統(tǒng)仿真學(xué)報(bào)， 2015， 27（6）：1288-1293. DOI：10.16182/j.cnki.joss.2015.06.020.

[9]許亞芳， 孫作雷， 曾連蓀， 等. 基于迭代觀測(cè)更新的移動(dòng)機(jī)器人視覺導(dǎo)航[J]. 信息與控制， 2015， 44（6）：739-744， 752. DOI：10.13976/j.cnki.xk.2015.0739.

[10]許亞芳， 孫作雷， 曾連蓀， 等. 基于激光掃描和迭代貝葉斯策略的定位[J]. 計(jì)算機(jī)工程與設(shè)計(jì)， 2015， 36（12）：3333-3338. DOI：10.16208/j.issn.1000-7024.2015.12.033.

[11]CUI Mingyue， LIU Wei， LIU Hongzhao. Unscented Kalman Filter-based adaptive tracking control for wheeled mobile robots in the presence of wheel slipping[C]//2016 12th World Congress on Intelligent Control and Automation （WCICA）， Guilin， June 12-15， 2016：3335-3340. DOI：10.1109/WCICA.2016.7578636.

[12]SENADHEERA S D A P， ABEYKOON A M H S. Sensorless terrain estimation for a wheeled mobile robot[C]//2017 IEEE International Conference on Industrial and Information Systems （ICIIS）， Peradeniya， 2017：1-6. DOI：10.1109/ICIINFS.2017.8300422.

[13]MAZULINA V， LITVINOV Y， BUSHUEV A. Developing a movement algorithm for a wheeled robot moving over a rough terrain[C]//2016 8th International Congress on Ultra Modern Telecommunications and Control Systems and Workshops （ICUMT）， Lisbon， 2016：5-9. DOI：10.1109/ICUMT.2016.7765224.

[14]周波， 戴先中， 韓建達(dá). 基于激光掃描的移動(dòng)機(jī)器人3D室外環(huán)境實(shí)時(shí)建模[J]. 機(jī)器人， 2012， 34（3）：321-328， 336. DOI：10.3724/SP.J.1218.2012.00321.

[15]YIN Xiaohong， ZHAO Han. On a new sectionalized motion control strategy for automated guided vehicles：modeling and simulation validation[J]. International Journal of Advanced Manufacturing Technology， 2013， 69：637-646.

[16]ROJAS-CONTRERAS F G， CASTILLO-LOPEZ A I， FRIDMAN L， et al. Trajectory tracking using continuous sliding mode algorithms for differential drive robots[C]//2017 IEEE 56th Annual Conference on Decision and Control （CDC）， Melbourne， Australia， VIC， 2017：6027-6032.

[17]ZDESˇAR A， SˇKRJANC I， KLANCˇAR G. Visual trajectory-tracking model-based control for mobile robots[J]. International Journal of Advanced Robotic Systems， 2013， 10：1. DOI：10.5772/56757.

[18]趙霞， 姜玉憲， 吳云潔， 等. 基于多模態(tài)滑模的快速非奇異終端滑?？刂芠J]. 北京航空航天大學(xué)學(xué)報(bào)， 2011， 37（1）：110-113. DOI：10.13700/j.bh.1001-5965.2011.01.027.

[19]任孝平， 蔡自興. 基于阿克曼原理的車式移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)建模[J]. 智能系統(tǒng)學(xué)報(bào)， 2009， 4（6）：534-537. DOI：10.3969/j.issn.1673-4785.2009.06.011.

[20]梁家榮， 譚紅艷， 樊仲光. 廣義系統(tǒng)的快速終端滑?？刂芠J]. 電子科技大學(xué)學(xué)報(bào)， 2011， 40（1）：11-15. DOI：10.3969/j.issn.1001-0548.2011.01.002.

[21]中華人民共和國(guó)交通部. 海港通用碼頭建設(shè)標(biāo)準(zhǔn)[S]. 北京：中華人民共和國(guó)交通部， 1996.

（編輯 趙勉）