基于反步法的多移載工裝協同作業編隊控制策略

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

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

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

合理的編隊控制器能夠有效降低隊列偏差所導致的作業過程中的內力.由于作業環境的擾動和移載工裝的運動誤差影響，由多移載工裝組成的多智能體系統魯棒性和穩定性較低，并且具有非線性的特點，而反步法適用于非線性不確定系統的控制，其采取分布式的控制器策略，對多智能體中的每個子系統設計運動控制器[3].Tian等[4]提出了干擾情況下二階多智能系統的反步法控制方法.Liu等[5]在經典的反步法基礎之上，提出有限時間觸發方法，減小了通信負擔.Xue等[6]則利用滑模控制將狀態觀測與反步法進行結合，設計了一種自適應反步滑模控制方法.

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

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

1 基于反步法的編隊控制器設計

1.1 結合虛擬結構法和虛擬領航者法的隊形設計

在協同搬運機翼的過程中，移載工裝需要保持編隊隊形的剛性結構，以減小隊形變形所產生的誤差進而降低內力.編隊結構的隊列模型如圖1所示.

圖1 虛擬領航者引領下的剛性結構隊形Fig.1 Rigid structure formation guided by virtual navigator

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

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

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

(1)

(2)

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

(3)

(4)

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

針對編隊中各個跟隨者產生的運動誤差，設計基于反步法的分布式隊形控制器.反步法應用于非線性系統的一般形式為

(5)

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

(6)

(7)

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

(8)

(9)

2 移載工裝的運動軌跡規劃與運動狀態估計

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

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

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

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

(10)

(11)

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

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

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

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

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

2.2 移載工裝預期軌跡的追蹤控制器設計

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

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

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

vxccosθ+vycsinθ=v

(12)

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

(13)

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

根據圖7的幾何關系可得距離方程：

(14)

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

(15)

通過選取較為平緩的速度和合適的預瞄距離來選取相關的輸入控制量，從而確保對運動軌跡較好的追蹤效果，并得到虛擬領航者的規劃運動狀態，根據提出的隊形結構的幾何關系變化，進一步計算跟隨者的期望運動狀態.

2.3 基于自適應蒙特卡洛算法的位姿估計

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

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

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

(16)

(17)

(18)

而移載工裝的運動狀態的后驗可靠性估計為

(19)

(20)

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

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

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

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

3 仿真驗證

3.1 仿真環境搭建以及參數設計

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

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

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

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

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

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

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

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

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

3.2 仿真結果分析

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

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

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

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

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

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

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

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

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

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

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

(21)

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

(22)

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

圖18 反步法控制效果與參數選取關系Fig.18 Relationship between backstepping control effect and parameter selection

圖19 跟隨者理想運動軌跡設計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內收斂到期望的位姿，達到整體的編隊要求.

4 結語

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