基于反步法的多移載工裝協(xié)同作業(yè)編隊控制策略

劉禹銘， 趙 勇， 董正建， 王 平， 姬煜琦

(1. 上海交通大學(xué) 上海市復(fù)雜薄板結(jié)構(gòu)數(shù)字化制造重點實驗室，上海 200240;2. 上海飛機制造有限公司航空制造技術(shù)研究所，上海 201324)

飛機大部件裝配作為飛機部裝的核心環(huán)節(jié)，安全高效的部件運輸以及自動化高精度的工裝對接是整體工作完成的重要保障[1].但是傳統(tǒng)的固定平臺裝配導(dǎo)致工廠場地利用率低，整體裝配過程靈活性低，并且裝配設(shè)備的成本較高，若使用移載工裝進行協(xié)同裝配，則能讓工廠的場地利用率和裝配效率提高[2].目前大多采用物流作業(yè)小車作為作業(yè)的移載工裝，但是其運動精度較低.當各移載工裝由于誤差原因?qū)е聦嶋H運動位姿偏離期望運動位姿時，多移載工裝易使搬運件產(chǎn)生較大內(nèi)力進而影響裝配結(jié)果和產(chǎn)品最終的服役性能.

合理的編隊控制器能夠有效降低隊列偏差所導(dǎo)致的作業(yè)過程中的內(nèi)力.由于作業(yè)環(huán)境的擾動和移載工裝的運動誤差影響，由多移載工裝組成的多智能體系統(tǒng)魯棒性和穩(wěn)定性較低，并且具有非線性的特點，而反步法適用于非線性不確定系統(tǒng)的控制，其采取分布式的控制器策略，對多智能體中的每個子系統(tǒng)設(shè)計運動控制器[3].Tian等[4]提出了干擾情況下二階多智能系統(tǒng)的反步法控制方法.Liu等[5]在經(jīng)典的反步法基礎(chǔ)之上，提出有限時間觸發(fā)方法，減小了通信負擔.Xue等[6]則利用滑?？刂茖顟B(tài)觀測與反步法進行結(jié)合，設(shè)計了一種自適應(yīng)反步滑模控制方法.

期望運動狀態(tài)和實際運動狀態(tài)是反步法控制策略下的核心變量.對于多智能體的期望運動狀態(tài)設(shè)計依賴于編隊方法的設(shè)計，目前常用的編隊方法主要包括領(lǐng)航跟隨法[7]、人工勢場法[8]、基于行為控制法[9]、虛擬結(jié)構(gòu)法[10]和虛擬領(lǐng)航者法[11].而其中能夠保持隊列剛性結(jié)構(gòu)的方法主要為領(lǐng)航跟隨法和虛擬結(jié)構(gòu)法.領(lǐng)航跟隨法將多機器人作業(yè)過程中的編隊問題轉(zhuǎn)化為領(lǐng)航機器人和跟隨機器人的期望角度與距離之間的關(guān)系問題.虛擬結(jié)構(gòu)法的主要理念是將多機器人的隊形假設(shè)為虛擬的剛性結(jié)構(gòu)進行控制規(guī)律設(shè)計.該編隊方法下整體隊形輕便，形成穩(wěn)定隊列的收斂時間快，但系統(tǒng)穩(wěn)定性難以保證，魯棒性和靈活性較差[12].基于多移載工裝進行協(xié)同裝配時的剛性隊列要求和不同工廠環(huán)境下模塊化裝配的需要，將虛擬結(jié)構(gòu)法和虛擬領(lǐng)航者法相結(jié)合，以虛擬結(jié)構(gòu)的參考點作為虛擬領(lǐng)航者進行作業(yè)運動規(guī)劃.常用的運動規(guī)劃算法有人工勢場法[13]、A*算法[14]、遺傳算法[15]、粒子群優(yōu)化算法[16]等.人工勢場法的運動規(guī)劃兼容編隊控制和避障規(guī)劃的要求，且具有計算量小、規(guī)劃路徑平滑易于設(shè)計軌跡追蹤器的優(yōu)點，被廣泛運用于工程實踐中.編隊控制的另一個重要環(huán)節(jié)是實際運動狀態(tài)的估計，通過測量設(shè)備的定位與濾波算法的結(jié)合，實時校正估計實際位置.而針對移動機器人的定位領(lǐng)域，基于粒子濾波算法進行定位最為廣泛.粒子濾波以其具有多模態(tài)、統(tǒng)計特征隨機分布的特性，適用于位姿估計和實時目標追蹤.但是粒子濾波過程中重采樣會導(dǎo)致粒子退化的問題，針對這一問題，趙國旗等[17]將模糊理論引入粒子濾波來防止重采樣中出現(xiàn)的多模態(tài)失效的問題.Li等[18]針對粒子濾波結(jié)合引入基于庫爾貝克-萊布勒散度(Kullback-Leibler Divergence, KLD)進行采樣的方法，有效解決了粒子退化問題.

本文以反步法作為編隊控制策略的基礎(chǔ)，將虛擬結(jié)構(gòu)法和虛擬領(lǐng)航者法相結(jié)合，通過改進的人工勢場法和純軌跡跟蹤法設(shè)計虛擬領(lǐng)航者的期望運動位姿，解決了路徑規(guī)劃陷入局部最優(yōu)的問題，進一步根據(jù)隊形幾何關(guān)系推導(dǎo)得到實際跟隨者的期望運動位姿.結(jié)合雷達測量設(shè)備和自適應(yīng)蒙特卡洛定位(Adaptive Monte Carlo Localization, AMCL)算法估計實際運動位姿，降低了每個移載工裝的定位誤差值，基于跟隨誤差設(shè)計了隊形控制器，完成了隊列的協(xié)同作業(yè)控制.

1 基于反步法的編隊控制器設(shè)計

1.1 結(jié)合虛擬結(jié)構(gòu)法和虛擬領(lǐng)航者法的隊形設(shè)計

在協(xié)同搬運機翼的過程中，移載工裝需要保持編隊隊形的剛性結(jié)構(gòu)，以減小隊形變形所產(chǎn)生的誤差進而降低內(nèi)力.編隊結(jié)構(gòu)的隊列模型如圖1所示.

圖1 虛擬領(lǐng)航者引領(lǐng)下的剛性結(jié)構(gòu)隊形Fig.1 Rigid structure formation guided by virtual navigator

考慮以期望隊形的幾何中心作為編隊參考點，在該位置添加一臺同類型的虛擬移載工裝，即虛擬領(lǐng)航者.實際的移載工裝作為跟隨者，進行整體的編隊控制，領(lǐng)航跟隨的關(guān)系如圖2所示.其中，C為小車質(zhì)心，w為角速度.

圖2 領(lǐng)航者與跟隨者編隊位姿誤差分析Fig.2 Pose error analysis of leader and follower formation

假設(shè)領(lǐng)航者機器人i在全局坐標系中對應(yīng)的坐標為(xi,yi)，則由圖示的幾何關(guān)系可以得到全局坐標的變換關(guān)系：

(1)

(2)

由式(1)與(2)可以得到移載工裝期望位姿以及實際位姿.將兩式相減，得到誤差的相應(yīng)關(guān)系式并將其對時間求導(dǎo)，進一步獲得誤差關(guān)于時間的微分方程，從而根據(jù)隊列的運動誤差，設(shè)計隊形控制器.

(3)

(4)

1.2 基于反步法的跟隨者隊形控制器設(shè)計

針對編隊中各個跟隨者產(chǎn)生的運動誤差，設(shè)計基于反步法的分布式隊形控制器.反步法應(yīng)用于非線性系統(tǒng)的一般形式為

(5)

式中：fx(x)和gx(x)為與輸入變量x相關(guān)的函數(shù)；ux(x)為輸入控制量.根據(jù)已得到的誤差動態(tài)微分方程，將xe、ye兩項誤差值看作隊形控制器的子系統(tǒng)，并遞歸尋找對應(yīng)的虛擬控制，對于兩項輸入控制vj和wj，當選取vj作為虛擬控制輸入時：

(6)

(7)

當ye=0時，V1≤0，則xe在有限時間收斂至0.對于下一個子系統(tǒng)微分方程的虛擬控制量wj，設(shè)計取值為

(8)

(9)

2 移載工裝的運動軌跡規(guī)劃與運動狀態(tài)估計

2.1 基于改進人工勢場法的避障路線設(shè)計

從整體隊列控制器的設(shè)計過程可知，期望的運動狀態(tài)和實際的運動狀態(tài)的求取是控制的兩大關(guān)鍵.期望的運動狀態(tài)需要滿足避障、運動平滑、對不同作業(yè)環(huán)境規(guī)劃適應(yīng)性強的要求，而求取實際的運動狀態(tài)需要滿足實時性強、精度高的要求，通過兩者決定的運動誤差來設(shè)計可用的隊列控制器.整體的設(shè)計流程圖如圖3所示.

圖3 整體編隊控制器設(shè)計流程圖Fig.3 Designed flow chart of overall formation controller

根據(jù)編隊控制器的設(shè)計流程，首先進行虛擬領(lǐng)航者的期望運動路徑設(shè)計.基于實際工作環(huán)境掃描得到多移載工裝的作業(yè)空間，在作業(yè)空間中引入人工勢場，障礙物周圍分布斥力場，目標點周圍分布引力場，使得被控對象在合力作用下自主移動.設(shè)定移載工裝所在位置為q(x0,y0)，目標點位置為g(xend,yend)，定義目標距離函數(shù)為ρ(q,g)，q為起始點，g為目標點.在傳統(tǒng)的人工勢場中常用的引力與斥力場函數(shù)為

(10)

(11)

式中：η、ξ為選取的引力系數(shù)與斥力系數(shù)；ρ(q)為移載工裝距離障礙物最近點的距離；q0為基于移載工裝自身尺寸以及隊形等要素所決定的安全距離.而斥力場僅在針對移載工裝定義的安全距離內(nèi)生效，以達到更短的避障路線規(guī)劃.由于虛擬領(lǐng)航者位于隊列幾何中心，所以選取幾何外接圓半徑為安全距離的參考基準，能簡潔地處理編隊避障的問題，路徑規(guī)劃的場地適應(yīng)性和實時性都能滿足要求.

圖4 結(jié)合模擬退火法的人工勢場法算法流程圖Fig.4 Algorithm steps of APF method in combination with simulated annealing

使用上述介紹的改進人工勢場法，當虛擬領(lǐng)航者陷入局部最優(yōu)導(dǎo)致運動震蕩時，通過搜索下一路徑點進行跳出，如圖5所示.

圖5(c)與5(f)中的箭頭符號象征人工勢場中的勢場矢量，而圖5(d)中的U為合勢場的大小，沒有對應(yīng)的單位.可以看出，采用模擬退火法使得被控移載工裝在全局坐標系中跳出了局部最優(yōu)點，重新規(guī)劃路徑，最終到達了目標點.

圖5 模擬退火法逃出局部最優(yōu)示意圖Fig.5 Diagram of escaping from local optimum by simulated annealing

2.2 移載工裝預(yù)期軌跡的追蹤控制器設(shè)計

使用改進人工勢場法規(guī)劃虛擬領(lǐng)航者的避障路徑之后，需要設(shè)計對應(yīng)的軌跡追蹤控制器保障移載工裝的循跡作業(yè).設(shè)計軌跡跟蹤器需要先構(gòu)建移載工裝的運動學(xué)模型.實際作業(yè)環(huán)境下的移載工裝的運動可忽略豎直方向上的運動，因此將被控對象的運動空間設(shè)計成二維平面.基于實際工況，在控制移載工裝的過程中，根據(jù)已有移載工裝的實際參數(shù)和控制變量，輸入對應(yīng)的移載工裝角速度和線速度，并將移載工裝簡化為圖6所示的差速輪驅(qū)動模型，圖中：XL和YL為小車以質(zhì)心C所建立的坐標系X軸與Y軸；r為小車差速輪半徑；2Ra為小車的寬度；p為小車車輪車軸的中心點，D為小車質(zhì)心C離輪軸中心的距離.

根據(jù)圖6建立的運動學(xué)模型，假定移載工裝為差速輪，其對應(yīng)整體工裝的控制量為[vw]T，即運動速度v和決定方向的角速度w.而相對應(yīng)的狀態(tài)變量為[xyθ]T，即全局坐標系下的x和y以及行駛方向與X軸正半軸的夾角.在圖6中，移載工裝的質(zhì)心在平面坐標系下的速度為vc，其在X,Y上的分量為vxc與vyc，則通過速度分解：

圖6 移載工裝簡化的運動學(xué)模型示意圖Fig.6 Schematic of simplified kinematic model of transfer tooling

vxccosθ+vycsinθ=v

(12)

通過簡化模型幾何關(guān)系可以得到差速輪模型的移載工裝運動方程，寫作整體系統(tǒng)方程形式:

(13)

圖7 純軌跡跟蹤法的幾何關(guān)系示意圖Fig.7 Geometric relationship schematic of pure pursuit tracking method

根據(jù)圖7的幾何關(guān)系可得距離方程：

(14)

并得到相關(guān)曲率的表達式和角速度控制方程：

(15)

通過選取較為平緩的速度和合適的預(yù)瞄距離來選取相關(guān)的輸入控制量，從而確保對運動軌跡較好的追蹤效果，并得到虛擬領(lǐng)航者的規(guī)劃運動狀態(tài)，根據(jù)提出的隊形結(jié)構(gòu)的幾何關(guān)系變化，進一步計算跟隨者的期望運動狀態(tài).

2.3 基于自適應(yīng)蒙特卡洛算法的位姿估計

移載工裝的實際位姿是隊形偏差計算的另一個關(guān)鍵元素.若直接采用測量設(shè)備進行定位獲取實際位姿，傳感器的誤差會直接影響定位的結(jié)果，進而導(dǎo)致隊形偏差計算失準.而結(jié)合濾波算法修正觀測變量的定位方法可以提高實際運動狀態(tài)的觀測精度，解決定位精度不夠的問題.本文構(gòu)建移載工裝的實時定位框架，采用自適應(yīng)蒙特卡洛定位算法進行實際位姿估計.

蒙特卡洛定位(Monte Carlo Localization, MCL)算法是一種適用于室內(nèi)較小地圖情況下的全局定位算法.該算法基于概率進行定位計算，使用粒子表征移載工裝所處位置的概率大小，初始時刻粒子平均分布在全局地圖上，在移載工裝運動作業(yè)時，粒子與移載工裝同步運動，并根據(jù)移載工裝的位姿篩選出與移載工裝運動狀態(tài)一致的粒子.在一段時間后粒子完成收斂時，當前粒子即表征移載工裝的真實位姿估計，若粒子收斂后表征的位姿是錯誤的，該算法無法做到糾偏和恢復(fù).當粒子數(shù)目較少時，丟失真實位姿附近的粒子的情況常有發(fā)生.而AMCL算法的引入有效地規(guī)避了這一問題.自適應(yīng)蒙特卡洛定位算法是蒙特卡洛定位算法的一種改進.AMCL通過KLD重采樣的方法注入了新的隨機粒子，減小粒子運算量的同時，避免移載工裝“綁架問題”的出現(xiàn).

首先針對移載工裝的運動狀態(tài)和觀測狀態(tài)進行建模，其狀態(tài)方程為

(16)

(17)

(18)

而移載工裝的運動狀態(tài)的后驗可靠性估計為

(19)

(20)

式中：Mχ為粒子集的數(shù)目閾值；k′為非空粒子狀態(tài)空間的個數(shù)；ε與β分別為真實分布與預(yù)估分布的最大誤差與正態(tài)分布的分位數(shù).根據(jù)KLD采樣的流程可以看出，當粒子對整體地圖覆蓋程度高時，采樣的粒子集數(shù)目降低，可以減小實時定位過程中的運算量；而粒子對地圖覆蓋程度較低時，粒子數(shù)目提高，在誤差協(xié)方差較低的情況下通過隨機引入粒子的方法防止重采樣過程中粒子退化的問題.如圖8所示，AMCL算法運用于移載工裝全局定位并最終收斂，收斂粒子表征移載工裝的估計位姿.

圖8 AMCL算法的定位收斂過程Fig.8 Location convergence process of AMCL algorithm

本文通過融合移載工裝的里程計與激光雷達的傳感器信息，將移載工裝運動模型和觀測模型代入AMCL算法，提出合理的粒子提議分布來逼近移載工裝的位姿，其具體估計流程架構(gòu)如圖9所示.

圖9 AMCL算法運用于移載工裝協(xié)同作業(yè)的定位流程Fig.9 Positioning process of AMCL algorithm applied to collaborative operation of mobile robot

3 仿真驗證

3.1 仿真環(huán)境搭建以及參數(shù)設(shè)計

通過在ROS-Gazebo中建立與現(xiàn)場環(huán)境相關(guān)的實際地圖和差速輪移載工裝模型，并進一步將實際地圖二值化，得到如圖10所示的場景圖，將規(guī)劃的路徑點與控制信號發(fā)送給移載工裝進行編隊位移控制.并在ROS中構(gòu)建在該場景下多移載工裝的仿真模型.

圖10 仿真作業(yè)空間建模Fig.10 Construction of workspace simulation environment

根據(jù)搭建好的二值地圖，通過改進人工勢場法來規(guī)劃虛擬領(lǐng)航者和跟隨者的期望路徑.其中載工裝的具體參數(shù)以及隊列期望距離如表1所示，導(dǎo)航方式為激光定位與第一構(gòu)建，并得到相應(yīng)的虛擬結(jié)構(gòu)剛性隊列的幾何中心路徑圖.

表1 移載工裝技術(shù)參數(shù)Tab.1 Technical parameter of transfer tooling

在移載工裝的運動空間中建立引力、斥力場，選取的引力系數(shù)η=1/70，斥力系數(shù)ξ=8/7，虛擬領(lǐng)航者與作業(yè)空間中障礙物的安全距離根據(jù)隊列距離與自身尺寸設(shè)置為35 cm.作業(yè)空間內(nèi)的總體引力場、斥力場以及總勢場如圖11所示，圖中：Urep為作業(yè)空間內(nèi)的斥力；Uatt為作業(yè)空間內(nèi)的引力.

圖11 移載工裝作業(yè)空間內(nèi)勢場變化圖Fig.11 Variation diagram of potential field in workspace of multiple mobile robots

通過給定參數(shù)的移載工裝，在Simulink中建立符合標準的基于差速輪驅(qū)動的移載工裝運動模型，并對期望軌跡通過純跟蹤法進行追蹤，得到虛擬領(lǐng)航者相應(yīng)的期望速度與角速度如圖12所示，其中設(shè)定預(yù)瞄距離下限值為5 cm.

圖12 差速輪移載工裝運動模型Fig.12 Motion model of differential wheel transfer tooling

對于整體的測量系統(tǒng)，采用車載激光雷達的出廠誤差作為測量誤差，結(jié)合實時控制的運動方程產(chǎn)生的誤差進行濾波從而對實際位姿與運動狀態(tài)進行估計與修正.其中距離測量誤差的初始提議分布為N(μ1，σ1) (μ1為均值，σ1為方差)，角度測量誤差的初始提議分布為N(μ2，σ2) (μ2為均值，σ2為方差)，均假設(shè)為高斯分布，協(xié)方差與均值由表1參考獲得.得到實際跟隨者移載工裝的狀態(tài)變量最優(yōu)值后，再通過反步法設(shè)定的控制策略，進行隊列的糾偏仿真.

3.2 仿真結(jié)果分析

在進行避障路徑規(guī)劃中使用傳統(tǒng)人工勢場法，即不引入模擬退火法逃離局部最優(yōu)點時，在目標點靠近障礙物的過程里，移載工裝會陷入勢場中的局部最優(yōu)點而小距離震蕩，最終無法到達目標點.移載工裝在勢場中停滯情況與整體路徑如圖13所示.

圖13 傳統(tǒng)人工勢場法下失敗的路徑規(guī)劃Fig.13 Failed path planning in traditional artificial potential field method

結(jié)合模擬退火法對陷入局部最優(yōu)的情況進行了改進.設(shè)定停滯震蕩的距離判定范圍為3 cm，并進行模擬退火法進行跳脫，其改進后路線如圖14所示.

圖14 改進人工勢場法成功規(guī)劃目標路徑Fig.14 Target path planed by improved artificial potential field method

設(shè)定預(yù)定路徑之后，使目標小車模型對路徑進行了軌跡追蹤，保持領(lǐng)航者速度10 cm/s勻速行駛進行編隊控制，通過計算得到的輸入控制量如圖15所示，圖中：T為控制時間.

圖15 虛擬領(lǐng)航者輸入運動變量Fig.15 Input movement variables of virtual pilot

確定系統(tǒng)輸入控制量后，使用Simulink搭建的移載工裝純軌跡跟蹤法模型進行軌跡追蹤仿真，得到圖16所示的追蹤效果和對應(yīng)誤差圖像，圖中E為移載工裝跟蹤運動誤差.仿真結(jié)果表明，輸入控制的角速度變化較平緩，追蹤偏差逐漸收斂于2.2 cm以內(nèi)，滿足追蹤的需求.

圖16 純軌跡跟蹤下的期望軌跡與誤差Fig.16 Desired trajectory and error in pure trajectory tracking

在仿真環(huán)境中讀取移載工裝的實際位姿，以激光雷達作為測量設(shè)備.根據(jù)蒙特卡洛定位以及自適應(yīng)蒙特卡洛定位方法估計移載工裝的實際位姿，并與直接使用激光雷達測量的方法進行對比，結(jié)果如圖17所示，圖中：Eθ為角度追蹤誤差；Edis為距離跟蹤誤差.對比可得，使用自適應(yīng)蒙特卡洛定位的方法可以將角度誤差降低到0.01° 以下，比直接使用測量設(shè)備的0.5° 測量誤差有較大降低.距離方面也將誤差收斂在0.8 cm以內(nèi)，兩者的誤差協(xié)方差系數(shù)也相較于MCL定位算法和直接使用測量設(shè)備進行定位的誤差協(xié)方差系數(shù)小.

圖17 自適應(yīng)蒙特卡洛定位方法誤差與其他方法誤差對比Fig.17 Comparison of error of adaptive Monte Carlo localization method and other methods

利用反步法設(shè)計的控制策略對整體隊形進行保持.對于反步法中k1和k2參數(shù)的選取，由于4臺實際跟隨者關(guān)于虛擬領(lǐng)航者幾何對稱，基于誤差的控制策略可選取相同參數(shù)，所以k1和k2參數(shù)只需要對單臺跟隨者進行設(shè)計，引入表征控制效果的指標：

(21)

式中：Δx、Δy及Δθ為實際運動與期望運動的誤差值；α為比例系數(shù)，取60，并將符合收斂的參數(shù)域中的指標進行歸一化：

(22)

式中：Si為第i臺移載工裝的評價指標歸一化結(jié)果；si為第i臺評價指標結(jié)果；S為多移載工裝評價指標之和.通過仿真得到不同參數(shù)變化情況下，移載工裝在跟隨過程中的評價指標變化情況，如圖18所示，并選取指標極大值位置的參數(shù)值為[1.02, 1.47]，即反步法中的控制參數(shù)[k1,k2], 紅點為極大值.

圖18 反步法控制效果與參數(shù)選取關(guān)系Fig.18 Relationship between backstepping control effect and parameter selection

圖19 跟隨者理想運動軌跡設(shè)計Fig.19 Ideal movement trajectory design of follower

表2 跟隨機器人三方向追蹤時間Tab.2 Track time in three directions of robot follower

圖20 跟隨者誤差指標隨時間變化曲線Fig.20 Follower error metric versus time

可以看出，在控制量有一定的擾動下，移載工裝依然能夠在6.2 s內(nèi)收斂到期望的位姿，達到整體的編隊要求.

4 結(jié)語

針對多移載工裝協(xié)同作業(yè)的控制問題，提出了一種基于反步法的多移載工裝協(xié)同作業(yè)編隊控制策略.建立了移載工裝的運動學(xué)模型，提出了基于改進人工勢場法和純軌跡追蹤法的期望運動狀態(tài)規(guī)劃方法，以及基于自適應(yīng)蒙特卡洛定位的實際運動狀態(tài)估計方法，與直接使用測量設(shè)備相比，角度誤差由 0.5° 降低至 0.01°，位置誤差由2 cm降低至 0.8 cm，精度提高.構(gòu)建了仿真模型進行驗證，隊形誤差在6.2 s內(nèi)達到收斂，滿足多移載工裝協(xié)同作業(yè)的條件.