協(xié)同式停車機(jī)器人的控制方法研究

高銘洋

（紐約大學(xué) 工學(xué)院，紐約 11201）

0 引言

到大自然中動(dòng)物協(xié)同工作的啟發(fā)，人們發(fā)現(xiàn)多機(jī)器人協(xié)同工作可以擴(kuò)展機(jī)器人的功能，并使機(jī)器人更有序、更高效、更靈活完成更復(fù)雜的工作[1,2]。Zhao等人采用多移動(dòng)機(jī)器人實(shí)現(xiàn)了大型構(gòu)件協(xié)同加工，有效的提高了生產(chǎn)效率[3,4]。Krid等人設(shè)計(jì)的多機(jī)器人協(xié)同搬運(yùn)系統(tǒng)能搬運(yùn)超長(zhǎng)、超重物體[5]。研究表明，多機(jī)器人協(xié)同搬運(yùn)不僅可以增加負(fù)載量，還能使搬運(yùn)操作更加靈活[6]。

停車機(jī)器人將自動(dòng)導(dǎo)航車（AGV）技術(shù)與汽車搬運(yùn)技術(shù)相結(jié)合，能夠靈活、智能、高效的按照預(yù)定軌跡將汽車安全停放至停車位[7]。而雙協(xié)同式停車機(jī)器人將多機(jī)器人協(xié)同技術(shù)和停車機(jī)器人技術(shù)相結(jié)合，可以利用其靈活、可重構(gòu)的特點(diǎn)，通過獨(dú)立-協(xié)同工作模式的切換，更加靈活的完成停車任務(wù)。

高精度協(xié)同控制和協(xié)同搬運(yùn)的穩(wěn)定性等問題一直是雙協(xié)同式機(jī)器人的技術(shù)難題。常見的協(xié)同控制方法如領(lǐng)航-跟隨法，其主要思路為令一領(lǐng)航機(jī)器人負(fù)責(zé)整個(gè)機(jī)器人編隊(duì)的導(dǎo)航。隊(duì)內(nèi)的其他機(jī)器人根據(jù)領(lǐng)航機(jī)的行動(dòng)方式進(jìn)行相應(yīng)的行動(dòng)。Koshi等人采用“一輪一機(jī)”的方式使用領(lǐng)航-跟隨法對(duì)小汽車進(jìn)行協(xié)同搬運(yùn)[8]。Endo等人令跟隨機(jī)器人通過與領(lǐng)航機(jī)器人和被搬運(yùn)物體間的作用力來判斷領(lǐng)航機(jī)軌跡，使得跟隨機(jī)無需與領(lǐng)航機(jī)進(jìn)行通信，大大降低了通訊成本[9]。吳垠等人采用不依賴搬運(yùn)器運(yùn)動(dòng)模型的模糊控制方法，分別控制領(lǐng)航機(jī)的軌跡和跟隨機(jī)與領(lǐng)航機(jī)的位姿差[10]。Wu等人使用載著攝像機(jī)的牽引車進(jìn)行定位，并對(duì)重型車輛的搬運(yùn)機(jī)器人進(jìn)行領(lǐng)航[11]。Sieber將人也加入機(jī)器人協(xié)同搬運(yùn)的系統(tǒng)中，將人視為領(lǐng)航者，而機(jī)器人視為跟隨者。這樣，人可以對(duì)處于搬運(yùn)系統(tǒng)中的機(jī)器人進(jìn)行實(shí)時(shí)引導(dǎo)[12]。其他文獻(xiàn)主要應(yīng)用分布式控制[13]。Eberhard等采取分布式模型預(yù)測(cè)控制，利用預(yù)測(cè)誤差來規(guī)劃合適的軌跡，實(shí)現(xiàn)了平滑的運(yùn)輸性能[14,15]。He等在分布式控制中通過降低控制算法所需數(shù)據(jù)減小了通訊成本[16]，以及在非建模環(huán)境下采用分布式控制模式，同時(shí)結(jié)合阻抗控制等多種柔性控制器來進(jìn)行動(dòng)態(tài)避障，并增加計(jì)算速度[17]。還有文獻(xiàn)采用集中式架構(gòu)。Ebel等設(shè)計(jì)了三層集中控制架構(gòu)，用于規(guī)劃并跟蹤每個(gè)機(jī)器人的運(yùn)動(dòng)軌跡[18]。Hichri等通過規(guī)劃并賦予不同的機(jī)器人在時(shí)間和空間上互不沖突的任務(wù)來達(dá)成多機(jī)器人的協(xié)同控制[19]。還有一些協(xié)同搬運(yùn)方法采用同步反饋而非與外界通信的方式。Obregon等人通過指定被搬運(yùn)物體的運(yùn)動(dòng)，來讓每個(gè)機(jī)器人根據(jù)物體預(yù)設(shè)位置進(jìn)行特定運(yùn)動(dòng)，達(dá)到協(xié)同搬運(yùn)的目的[20]。Wang等設(shè)置領(lǐng)航者來引導(dǎo)物體運(yùn)動(dòng)，而其他每個(gè)搬運(yùn)機(jī)器人通過感知物體的運(yùn)動(dòng)進(jìn)行協(xié)同搬運(yùn)[21,22]。Loh等令每個(gè)機(jī)器人靠傳感器感知其他機(jī)器人及被搬運(yùn)物體的狀態(tài)變化并作出反饋[23]。Gronemeyer等人采用人工力場(chǎng)算法避免多機(jī)器人協(xié)同時(shí)的碰撞問題[24]。從以上研究結(jié)果可以看出，協(xié)同時(shí)機(jī)器人間的通訊成本、協(xié)同運(yùn)動(dòng)時(shí)隊(duì)形的魯棒性以及每個(gè)機(jī)器人的控制精度是研究協(xié)同搬運(yùn)最需要解決的問題。

針對(duì)以上問題，本文將提出一種基于領(lǐng)航跟隨法和模型預(yù)測(cè)控制算法的控制策略（LFMPC，Lead follow -model predictive control）。領(lǐng)航機(jī)通過機(jī)器人的運(yùn)動(dòng)模型預(yù)測(cè)機(jī)器人在特定時(shí)間內(nèi)的運(yùn)動(dòng)方式及所處位置，并求解出一個(gè)最優(yōu)控制使得領(lǐng)航機(jī)沿預(yù)設(shè)路徑行進(jìn)。而跟隨機(jī)實(shí)時(shí)通信獲得領(lǐng)航機(jī)位置及其即將進(jìn)行的運(yùn)動(dòng)，并用上述模型預(yù)測(cè)的方法與領(lǐng)航機(jī)保持特定位姿，實(shí)現(xiàn)跟隨領(lǐng)航機(jī)的運(yùn)動(dòng)。由于本文的控制對(duì)象為雙機(jī)器人協(xié)同式停車機(jī)器人，故優(yōu)先采用領(lǐng)航-跟隨的協(xié)同控制策略。且由于領(lǐng)航-跟隨法與領(lǐng)航機(jī)的行動(dòng)軌跡無關(guān)，因此可以使領(lǐng)航機(jī)引領(lǐng)下的搬運(yùn)系統(tǒng)在平面內(nèi)更自由的進(jìn)行運(yùn)動(dòng)[10]。另外，模型預(yù)測(cè)控制具有魯棒性強(qiáng)、通信頻率要求不高，靈活利用狀態(tài)與輸入的約束，從而可以更好地處理有約束的控制問題[25]。同時(shí)由于模型預(yù)測(cè)控制對(duì)模型精度要求不高，因而可以彌補(bǔ)非線性模型線性化時(shí)造成的模型失真。Eberhard等人采用模型預(yù)測(cè)控制算法，僅對(duì)傳統(tǒng)的“前輪轉(zhuǎn)向+后輪驅(qū)動(dòng)”式搬運(yùn)機(jī)器人進(jìn)行協(xié)同控制仿真研究，而本文控制的對(duì)象為運(yùn)動(dòng)更加靈活的協(xié)同式停車機(jī)器人，各使用四個(gè)全向輪驅(qū)動(dòng)。而且，本文對(duì)所提出的LFMPC控制策略完成了實(shí)驗(yàn)驗(yàn)證。

本文結(jié)構(gòu)主要分為六個(gè)部分：第二節(jié)介紹雙協(xié)同式停車機(jī)器人的工作模式和性能指標(biāo)；第三節(jié)將對(duì)LFMPC控制算法進(jìn)行詳細(xì)論述；第四節(jié)將LFMPC算法應(yīng)用在停車入庫(kù)場(chǎng)景中，并對(duì)此進(jìn)行仿真分析；第五節(jié)為實(shí)驗(yàn)驗(yàn)證；最后為本文結(jié)論。

1 停車機(jī)器人工作模式及性能指標(biāo)

本文所研究的協(xié)同式停車機(jī)器人基于深圳閃移開發(fā)的Combo停車機(jī)器人，其產(chǎn)品圖片如圖1所示，搬運(yùn)方式如圖2所示，其主要性能指標(biāo)如表1所示。協(xié)同式停車機(jī)器人由兩套可獨(dú)立運(yùn)行的夾臂式搬運(yùn)器組成。非工作狀態(tài)下，兩個(gè)機(jī)器人在夾臂收縮狀態(tài)下合并為一體。其工作模式為：合并狀態(tài)的機(jī)器人獨(dú)立運(yùn)行至待停車輛的底部，然后兩機(jī)器人分離并分別運(yùn)動(dòng)至與小轎車前/后輪軸處；雙機(jī)器人夾臂同步展開并夾起車輪，然后協(xié)同運(yùn)動(dòng)至車位處，之后雙機(jī)夾臂同步松開完成自動(dòng)停車；停車任務(wù)完成后，雙機(jī)器人再次合并，并運(yùn)行至下一待停車輛或存放點(diǎn)。與其他傳統(tǒng)的停車機(jī)器人比較，這類雙協(xié)同式停車機(jī)器人無需載車板、轉(zhuǎn)臺(tái)等輔助設(shè)施，可直接應(yīng)用于現(xiàn)有停車場(chǎng)。采用雙機(jī)協(xié)同工作模式，不僅減小了停車機(jī)器人的體積，從而降低了占地面積，也使其運(yùn)動(dòng)、轉(zhuǎn)運(yùn)更加靈活。

圖1 雙協(xié)同式停車機(jī)器人圖片

圖2 雙協(xié)同停車機(jī)器人搬運(yùn)方式

表1 停車機(jī)器人協(xié)同搬運(yùn)時(shí)的性能指標(biāo)

2 領(lǐng)航跟隨-模型預(yù)測(cè)控制

2.1 單搬運(yùn)器的模型預(yù)測(cè)控制方法

由于每個(gè)搬運(yùn)器均由四個(gè)全向輪驅(qū)動(dòng)，因此通過調(diào)節(jié)機(jī)器人的四個(gè)全向輪轉(zhuǎn)速及方向可任意調(diào)節(jié)機(jī)器人整體的運(yùn)動(dòng)速度大小v、速度方向偏角δ以及自轉(zhuǎn)角速度ω。因而可以進(jìn)行任意方向的平移運(yùn)動(dòng)和以任意點(diǎn)為圓心的阿克曼轉(zhuǎn)向運(yùn)動(dòng)。其關(guān)系為：

xr、yr為機(jī)器人阿克曼轉(zhuǎn)向圓心距機(jī)器人中心處的x與y坐標(biāo)差，ωr為機(jī)器人阿克曼轉(zhuǎn)向角速度。

由于可以通過指定機(jī)器人的控制量來任意控制機(jī)器人的速度大小、方向和自轉(zhuǎn)角速度，則不妨將機(jī)器人速度大小方向和自轉(zhuǎn)速度作為系統(tǒng)輸入量，位姿為狀態(tài)量，即：

狀態(tài)量與輸入量的關(guān)系為：

對(duì)于某一時(shí)刻的任意控制量或輸入量s，均有s=s*+Δs，s*為在該時(shí)刻的理想值，而s為實(shí)際值，Δs為實(shí)際值與理想值的偏移量。由于狀態(tài)量和輸入量的實(shí)際值和理想值均應(yīng)滿足式(6)，因此：

將機(jī)器人在一段時(shí)間內(nèi)控制量保持恒定的運(yùn)動(dòng)狀態(tài)稱為定常運(yùn)動(dòng)，那么上述平移和阿克曼轉(zhuǎn)向兩種運(yùn)動(dòng)均為定常運(yùn)動(dòng)。對(duì)于任意運(yùn)動(dòng)軌跡均可拆分或近似拆分為若干段定常運(yùn)動(dòng)（平移或轉(zhuǎn)向）。對(duì)于每一段定常運(yùn)動(dòng)，機(jī)器人的輸入量的理想值保持不變。

模型預(yù)測(cè)控制的目標(biāo)是通過控制Δu(k)的值能使控制間隔時(shí)間tc=n*t里偏差y(k)盡量小，同時(shí)調(diào)整量Δu(k)盡量小，故定義損失函數(shù)為：

Q和R分別為y(k)和Δu(k)的權(quán)重。將式(9)代入式(10)得：

可以使用二次規(guī)劃方法求解Δu(k)的最優(yōu)值，然后將u(k)=u(k)*+Δu(k)作為ρ(k)的采樣時(shí)刻后tc時(shí)間內(nèi)的輸入，這樣可以實(shí)現(xiàn)對(duì)任意標(biāo)準(zhǔn)運(yùn)動(dòng)的控制。

2.2 雙搬運(yùn)器協(xié)同控制方法

基于單搬運(yùn)器的模型預(yù)測(cè)控制方法，本節(jié)論述跟隨機(jī)如何與領(lǐng)航機(jī)進(jìn)行協(xié)同搬運(yùn)。雙搬運(yùn)器協(xié)同轉(zhuǎn)彎方式為雙搬運(yùn)器同時(shí)以其中垂線上一點(diǎn)為圓心進(jìn)行相同半徑的阿克曼轉(zhuǎn)向，如圖3所示，因此其輸入關(guān)系為：

圖3 雙搬運(yùn)器協(xié)同阿克曼轉(zhuǎn)向

運(yùn)動(dòng)中需要保持雙搬運(yùn)器的偏航角相同，同時(shí)跟隨機(jī)保持在領(lǐng)航機(jī)的正后方特定距離。即：

上述為跟隨機(jī)的理想狀態(tài)量ρ2*和控制量u2*與領(lǐng)航機(jī)的關(guān)系。跟隨機(jī)將通過與領(lǐng)航機(jī)通訊實(shí)時(shí)獲得領(lǐng)航機(jī)當(dāng)前的狀態(tài)量ρ1*和u1*控制量，并依據(jù)式(12)～式(15)計(jì)算出跟隨機(jī)當(dāng)前的參考狀態(tài)量ρ2*(k)和控制量u2*(k)。然后采用上小節(jié)中的模型預(yù)測(cè)控制方式，根據(jù)跟隨機(jī)該時(shí)刻的實(shí)際狀態(tài)量ρ2(k)計(jì)算出跟隨機(jī)在接下來tc時(shí)間里的最優(yōu)控制u2(k)。

2.3 雙搬運(yùn)器協(xié)同控制的仿真研究

本小節(jié)將利用Simulink環(huán)境對(duì)LFMPC控制方法進(jìn)行仿真。仿真場(chǎng)景為搬運(yùn)器即將進(jìn)行右轉(zhuǎn)彎，轉(zhuǎn)彎半徑為2m，標(biāo)準(zhǔn)速度為ρ*=2m/s。在初始狀態(tài)下領(lǐng)航機(jī)狀態(tài)偏離量分別為x=-1m，y=-0.05m，φ=0.12rad；跟隨機(jī)狀態(tài)偏離量為x=0.75m，y=0.1m，φ=-0.08rad。控制器參數(shù)為：

圖4、圖5分別為仿真過程中領(lǐng)航機(jī)和跟隨機(jī)的位姿偏差，表2為在不同初始狀態(tài)偏差下經(jīng)過轉(zhuǎn)彎運(yùn)動(dòng)前后的位姿偏差。由仿真結(jié)果可知，使用LFMPC控制能夠在一個(gè)轉(zhuǎn)彎運(yùn)動(dòng)中將偏差降低至原先的以內(nèi)，因此LFMPC控制能夠較好的完成定常運(yùn)動(dòng)的軌跡跟蹤與隊(duì)形保持任務(wù)，對(duì)外界環(huán)境及其他干擾等造成的運(yùn)動(dòng)偏差具有較強(qiáng)的魯棒性。

圖4 領(lǐng)航機(jī)位姿與預(yù)設(shè)軌跡的偏差

圖5 跟隨機(jī)位姿與預(yù)設(shè)軌跡的偏差

表2 停車機(jī)器人協(xié)同搬運(yùn)時(shí)的性能指標(biāo)

3 基于LFMPC 的停車方案和仿真

協(xié)同式停車機(jī)器人能夠準(zhǔn)確地將小汽車停入特定車位。本節(jié)將具體討論兩種不同停車入庫(kù)場(chǎng)景下的停車方案，然后采用Simulink環(huán)境對(duì)此場(chǎng)景進(jìn)行仿真，并對(duì)比在使用LFMPC控制策略與直接使用開環(huán)控制下的停車結(jié)果偏差。

3.1 仿真情景設(shè)定

本節(jié)將討論的車庫(kù)為較常見的T字型車位，對(duì)于這種車位主要有兩種停車方案。預(yù)設(shè)情景如圖6所示。設(shè)定車的預(yù)設(shè)初始位置的中軸線距離車位所在路邊1.2m，汽車中心距離車庫(kù)中垂線2m，車庫(kù)中垂線與道路中垂線交點(diǎn)距離預(yù)設(shè)泊車地點(diǎn)2.5m。停車方案一的具體步驟為：先前進(jìn)3.5m，然后以1.5m為半徑向側(cè)后方進(jìn)行90o阿克曼轉(zhuǎn)向運(yùn)動(dòng)，最后后退1m，第一種停車方案的具體步驟如圖7所示。停車方案二的具體步驟為先以4.83m為半徑向車庫(kù)側(cè)前方進(jìn)行45o的阿克曼轉(zhuǎn)向運(yùn)動(dòng)，再以同樣半徑向側(cè)后方進(jìn)行45o阿克曼轉(zhuǎn)向的倒車運(yùn)動(dòng)，最后倒車0.5m，第二種停車方案的具體步驟如圖8所示。接下來將對(duì)這兩種情景分別進(jìn)行仿真，設(shè)定領(lǐng)航機(jī)初始偏差為0.5m，初始y偏差為0.5m，姿態(tài)角偏差為0.2rad，跟隨機(jī)初始x偏差為-0.2m，y偏差為0.2m，姿態(tài)角偏差為-0.15rad。

圖6 情景預(yù)設(shè)

圖7 停車方案一

圖8 停車方案二

3.2 仿真實(shí)驗(yàn)結(jié)果

圖9、圖10分別為停車方案一使用LFMPC控制和完全開環(huán)控制的仿真結(jié)果對(duì)比；圖11、圖12為停車方案二在使用LFMPC控制和完全開環(huán)控制的仿真結(jié)果對(duì)比。采用停車方案一，用時(shí)約8s；采用停車方案二，則用時(shí)約12s。仿真結(jié)果表明無論采用哪種方案，使用LFMPC控制均可在整個(gè)過程的前2s便將狀態(tài)偏差縮減至原先偏差的5%以內(nèi)，而在整個(gè)過程結(jié)束時(shí)的狀態(tài)偏差基本歸零；而全程采用開環(huán)控制時(shí)狀態(tài)偏差或維持原先水平，或有所增加。因此LFMPC控制在外在干擾所造成的偏差較大的情況下依然能使得停車機(jī)器人較好的保持隊(duì)形，并將待停車輛準(zhǔn)確停在指定位置。

圖9 停車方案一開環(huán)偏差

圖10 停車方案一閉環(huán)偏差

圖11 停車方案二開環(huán)偏差

圖12 停車方案二閉環(huán)偏差

LFMPC控制在外在干擾所造成的偏差較大的情況下依然能使得停車機(jī)器人較好的保持隊(duì)形，并將待停車輛準(zhǔn)確停在指定位置。

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

本節(jié)將對(duì)自主研發(fā)的協(xié)同停車機(jī)器人使用LFMPC控制策略開展實(shí)驗(yàn)驗(yàn)證，并選擇深圳市某地下車庫(kù)作為實(shí)驗(yàn)場(chǎng)地，選用普通小轎車作為搬運(yùn)對(duì)象。該車庫(kù)路面大體平整，但局部凸凹不平，非常貼近于常見的小區(qū)地下車庫(kù)的地面。協(xié)同停車機(jī)器人停車路線如圖13所示，并采用上節(jié)中所述的停車方案一進(jìn)行停車入庫(kù)。實(shí)驗(yàn)結(jié)果如表3所示。

圖13 協(xié)同機(jī)器人停車路線

表3 實(shí)驗(yàn)結(jié)果

在入庫(kù)實(shí)驗(yàn)過程中，汽車無明顯晃動(dòng)，且基本沿規(guī)劃軌跡運(yùn)動(dòng)。實(shí)驗(yàn)結(jié)果表明：LFMPC控制能夠使機(jī)器人較好的適應(yīng)崎嶇路面與初始位置偏差等干擾，其位置偏差≤150mm，姿態(tài)偏差小于5o。

從實(shí)驗(yàn)結(jié)果可知：真實(shí)實(shí)驗(yàn)與仿真實(shí)驗(yàn)結(jié)果還存在一定誤差。誤差主要來源于兩個(gè)方面：第一、來源于搬運(yùn)機(jī)通過導(dǎo)航得到的狀態(tài)量和實(shí)際狀態(tài)量之間的偏差。LFMPC方法采用領(lǐng)航機(jī)和跟隨機(jī)的位姿作為狀態(tài)量進(jìn)行控制和反饋，因而導(dǎo)航的精準(zhǔn)程度將直接影響LFMPC方法反饋的精度，從而降低系統(tǒng)的魯棒性。第二、來自于在仿真實(shí)驗(yàn)時(shí)未考慮實(shí)際負(fù)載時(shí)領(lǐng)航機(jī)與跟隨機(jī)同時(shí)夾抱小轎車輪時(shí)互相之間的約束力，這一約束力會(huì)在一定程度上阻礙領(lǐng)航機(jī)與跟隨機(jī)自由調(diào)整位姿。實(shí)驗(yàn)結(jié)果表明，導(dǎo)航誤差和夾抱車輪時(shí)約束力所造成的偏差不會(huì)過多的影響LFMPC方法的魯棒性。

5 結(jié)語

1）提出和驗(yàn)證了領(lǐng)航跟隨-模型預(yù)測(cè)控制方法，適用于協(xié)同式停車機(jī)器人。

2）LFMPC控制方法對(duì)外在干擾具有較強(qiáng)的魯棒性，能讓雙協(xié)同式停車機(jī)器人在保持搬運(yùn)隊(duì)形。

3）雙協(xié)同式機(jī)器人進(jìn)行入庫(kù)時(shí)采用LFMPC方法能大大降低由初始位姿偏差、路面崎嶇等因素干擾所造成的偏差，實(shí)驗(yàn)所得的位置偏差≤50mm，姿態(tài)偏差小于5o。