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

高銘洋

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

0 引言

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

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

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

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

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

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

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

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

圖2 雙協(xié)同停車機器人搬運方式

表1 停車機器人協(xié)同搬運時的性能指標(biāo)

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

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

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

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

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

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

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

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

模型預(yù)測控制的目標(biāo)是通過控制Δu(k)的值能使控制間隔時間tc=n*t里偏差y(k)盡量小，同時調(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)的采樣時刻后tc時間內(nèi)的輸入，這樣可以實現(xiàn)對任意標(biāo)準(zhǔn)運動的控制。

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

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

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

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

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

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

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

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

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

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

表2 停車機器人協(xié)同搬運時的性能指標(biāo)

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

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

3.1 仿真情景設(shè)定

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

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

圖7 停車方案一

圖8 停車方案二

3.2 仿真實驗結(jié)果

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

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

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

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

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

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

4 實驗

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

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

表3 實驗結(jié)果

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

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

5 結(jié)語

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

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

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