雙機器人松協調裝配軌跡規劃方法研究

姚俊，李俊淵，李明富,b

(湘潭大學 a. 機械工程學院； b. 復雜軌跡加工工藝及裝備教育部工程研究中心，湖南 湘潭 411105)

0 引言

雙機器人裝配可以改善單機器人裝配工作空間有限、靈活度不夠等缺陷。在常見的雙機器人裝配過程中，雙機器人之間的位置姿態約束關系是動態變化的，所以雙機器人協調裝配過程為松協調。通常情況下，雙機器人裝配主要任務分為兩個階段：第一階段為抓取階段，即在有障礙的環境中主-從機器人各自抓取配件，繞過障礙物尋找一條避免碰撞干涉的路徑到達指定的裝配位置，為協調裝配做準備[1-3]；第二階段為松協調裝配階段，即主-從機器人在裝配體之間的約束下進行相對裝配運動[4-7]。

為了滿足協調裝配中雙機器人的軌跡連續性和約束關系，本文采用B樣條曲線對關節軌跡進行插值，并對插值引起的裝配誤差進行補償，以保證裝配體之間的約束和驅動器的平穩性。量子粒子群算法(QPSO)[8]具有尋優能力較好、能夠快速收斂到全局最優點、所需調整的參數較少、算法易于實現等優點，可以規劃出較平滑的關節軌跡，且優化效率較高。

1 雙機器人協調裝配運動學分析

1.1 雙機器人協調系統坐標系

對雙機器人協調系統中的機器人和裝配體分別建立坐標系，坐標系簡圖與實物圖分別如圖1(a)、圖1(b)所示。用i表示機器人代號，i=1,2；在雙機器人協調裝配系統中，夾持裝配體A的1號機器人為主機器人，夾持裝配體B的2號機器人為從機器人；{Ri}為主-從機器人的基坐標系；{Ei}為主-從機器人末端的坐標系；{WPi}為主-從機器人所夾裝配體的坐標系；{Pi}為裝配任務點的坐標系。

圖1 雙機器人協調裝配系統坐標系簡圖和實物圖

1.2 協調裝配誤差分析

為了適應機器人控制器對驅動器控制的非連續性和平滑性，需要對主機器人軌跡進行插值，而插值得到的軌跡不能保證主機器人嚴格地按照任務要求進行連續直線運動，進而產生裝配誤差。而細微的誤差會導致裝配體上產生巨大的力或者力矩，使裝配體產生變形，甚至損壞機器人。因此分析裝配誤差和規劃出滿足位置約束、姿態約束關系的雙機器人協調裝配軌跡是很有必要的。

對于軸孔裝配模型，軸孔裝配中軸線距離的最大值可以直觀地反映出兩裝配體軸線的偏離程度以及兩裝配體坐標原點的位置偏離程度。 在理想裝配情況下，{WP2}相對于{WP1}的z軸始終重合，裝配體A與裝配體B之間相對運動約束為裝配體A沿著{WP2}的z軸做直線運動；{WP2}相對于{WP1}的約束姿態用式(1)所示的Z-Y-X歐拉角矩陣WP1RWP2表示：

WP1RWP2=RZ(α*)RY(β*)RX(γ*)

(1)

式中α*、β*、γ*表示理想裝配情況下的{WP2}相對于{WP1}的Z-X-Y歐拉角。在實際裝配情況下，WP1RWP2的Z-Y-X歐拉角分別為α、β、γ；當前兩個裝配體之間姿態變換的歐拉角與理想歐拉角的差值取絕對值求和可以明顯地反映實際情況下與理想情況下兩裝配體之間的約束姿態的差別，裝配姿態誤差AtE的定義如式(2)所示：

AtE=|α-α*|+|β-β*|+|γ-γ*|

(2)

綜上所述，裝配位置誤差的定義為：裝配體A與裝配體B坐標系的原點到理想裝配軌跡線上距離的最大值；裝配姿態誤差的定義為：對當前裝配體A與裝配體B之間姿態變換的歐拉角與理想歐拉角差值的絕對值求和。在協調裝配過程中，為了降低裝配誤差，提高裝配成功率，主機器人主要實現直線插入運動，從機器人根據裝配約束關系對主機器人運動所產生的裝配誤差做補償運動。

2 基于B樣條的關節軌跡規劃模型

2.1 主-從機器人B樣條關節軌跡的確定

主機器人末端在時刻ti處經過一系列指定的笛卡兒空間任務點Pi(i=0,1,…,n1)，在關節空間中經過關節插值點qi,1，采用累積弦長參數化方法將時間節點ti歸一化為ui+k∈[uk,un1+k]。將ui+k代入關節軌跡表達式中，得到主機器人k次B樣條軌跡曲線滿足插值條件的n1+1個方程為：

(3)

B樣條曲線的控制點數為n1+k，為求解控制點，還需附加k-1個方程，可以由切矢邊界條件得到。設定主機器人在起始時刻的速度為vs1，加速度為as1，加加速度為js1；終止時刻的速度為ve1，加速度為ae1，加加速度為je1；當k=7時，附加方程如式(4)所示：

(4)

從機器人隨主機器人的運動變化，根據裝配約束條件確定從機器人末端的位姿，由逆運動學求得從機器人qi,2，得到從機器人經過的關節位置-時間節點序列。為了補償主機器人B樣條插值軌跡的誤差，在主機器人B樣條軌跡的時間節點區間內計算最大裝配位置誤差及此時刻主機器人的關節位置，并根據裝配約束條件確定此時刻從機器人末端的位姿，由逆運動學求得從機器人新增的關節插值點qi,new與ti,new,i=1,2,…,nnew，其中nnew為總共新增插值點的個數，則從機器人經過的關節位置-時間節點序列如式(5)所示：

(5)

式中函數sort(x)表示對x進行非遞減排序，并返回排序后的數組。隨后同主機器人B樣條關節軌跡確定方法，結合從機器人的歸一化時間、附加方程、時間節點向量求得從機器人的B樣條關節軌跡。

2.2 誤差補償策略

主機器人驅動裝配體A從裝配任務點Pi連續運動到Pi+1時，由于B樣條插補運動會產生一定的裝配位置誤差，由此產生的裝配阻力過大會使裝配失敗甚至損害重要部件。因此需要從機器人在裝配約束下對主機器人的B樣條關節軌跡進行補償，補償策略如下：

1) 根據主機器人的關節位置序列qi,1和時間序列ti(i=0,1,…,n1)，確定從機器人的末端位姿，由從機器人逆運動學確定相應的從機器人關節位置qi,2和時間序列ti(i=0,1,…,n2),n2=n1，令logo=0。

2) 在關節空間中利用從機器人關節軌跡的確定方法對從機器人經過的關節位置-時間節點序列進行插值，得到從機器人的關節軌跡；如果logo=0，則執行步驟3；否則輸出主-從機器人B樣條關節軌跡，算法終止。

3) 預設允許最大裝配位置誤差Ep,max和姿態誤差為Ea,max。遍歷從機器人B樣條軌跡的時間區間[ti,ti+1],i=0,1,…，n2-1內的最大裝配位置誤差和姿態誤差；若最大裝配位置誤差和姿態誤差都小于Ep,max和Ea,max，令logo=1，則執行步驟2)；若最大裝配位置誤差和姿態誤差不都小于Ep,max和Ea,max，則計算出此刻主機器人的關節位置，根據裝配約束關系確定此時刻的{WP2}，對該區間內主機器人B樣條插值產生的裝配誤差進行補償。采用逆運動學求得該時刻從機器人新增的插值點qi,new與相應時刻ti,new,i=1,2,…，nnew,其中nnew為新插值點總數；并更新從機器人經過的關節位置-時間節點序列；執行步驟2)。

3 基于QPSO的協調裝配軌跡優化

雙機器人裝配過程中，機械臂與障礙物之間以及機械臂與機械臂之間可能發生碰撞干涉。在實際操作中，通過人工示教操作可得到避障中間點，但是此避障中間點不一定是最優的避障位置，且會影響整個協調裝配任務的完成時間及軌跡的平滑性。

通過調整主-從機器人避障點的位置，可以調整整個裝配過程的時間、平均脈動以及判斷是否發生碰撞。因此確定優化變量如式(6)所示：

(6)

整個雙機器人裝配軌跡優化過程如圖2所示。首先，將已知的主-從機器人末端初始位置、過渡點、協調裝配序列點映射到關節空間；然后根據示教的避障中間點以及搜索范圍確定算法粒子搜索空間；隨后，將兩條軌跡進行B樣條插值；最后結合目標函數和罰函數，利用QPSO算法搜索主-從機器人最優或者接近較優的避障點，從而得到最優或者接近較優的雙機器人協調裝配軌跡。

圖2 雙機器人裝配軌跡優化過程示意圖

利用QPSO求解雙機器人協調裝配中多目標優化軌跡問題的算法，實現步驟如圖3所示。

圖3 QPSO算法偽代碼

4 仿真實例

4.1 仿真實驗條件

以兩個Staubli tx90工業機器人為實驗對象，雙機器人各需要避開1個障礙物后進行協調軸孔裝配為例。基于Open Inventor環境，利用VC++6.0編程軟件建立了雙機器人協調裝配的仿真平臺。主-從機器人的初始時刻的關節位置都為(-90°,-20°,130°,0°,65°,0°)；通過示教操作雙機器人得到主-從機器人的避障中間點處的關節位置都為(-90°，-45°，180°，0°，39°，0°)。設關節空間中對避障中間點處的關節搜索幅度δ=15，干涉檢測點間距L=2.5mm。選取主機器人基坐標系為世界坐標系，主-從機器人的關節軌跡均由7次B樣條曲線插值得到，在主-從機器人夾持工件的起始時刻和裝配結束時刻的速度、加速度和加加速度均為0。不失一般性，取約束放大系數kv1、ka1、kj1、kv2、ka2、kj2均為2.9，預設允許最大位置裝配誤差Ep,max為0.01mm，預設允許最大姿態裝配誤差Ea,max為0.001°，權衡3個評價指標對優化目標函數的影響程度，取ω1、ω2、ω3均為1。通過主機器人的逆運動學將裝配任務點轉換到主機器人的關節空間中，得到裝配任務的關節位置。從機器人經過的關節位置序列由雙機器人協調裝配軌跡規劃策略得到。

4.2 仿真結果及分析

利用量子粒子群算法求解雙機器人協調裝配軌跡多目標優化問題，取粒子數為100，迭代150次，得到適應度函數如圖4所示。

圖4 適應度函數值

由圖4可知，迭代初期適應度值快速下降，當迭代到第50次后，收斂速度減慢，逼近于最小值。采用QPSO算法規劃出雙機器人協調裝配軌跡的最短執行時間為ft=13.0017s；平均脈動為fj=3.8219。

在裝配過程中，假設將從機器人所夾持的裝配體B保持靜止，主機器人夾持的裝配體A相對于靜止的裝配體B作直線插入運動，所形成的裝配誤差為單機器人裝配誤差。單、雙機器人裝配位置誤差如圖5(a)所示，單、雙機器人裝配姿態誤差如圖5(b)所示。

由圖5可見，經過3個過渡點(單機器人位置、姿態誤差圖上的前3個誤差值過0點)后，雙機器人裝配位置誤差和裝配姿態誤差都接近于0。與單機器人裝配機器人誤差相比，雙機器人協調裝配誤差明顯小于單機器人裝配誤差。通過QPSO優化以及誤差補償得到的雙機器人B樣條關節軌跡驅動主-從機器人協調運動，可以使主-從機器人分別繞過障礙物、降低裝配誤差且以較優的軌跡高效地完成協調裝配作業，從而驗證了所提的雙機器人協調裝配軌跡規劃方法的有效性和可靠性。

圖5 單、雙機器人裝配誤差

5 結語

結合QPSO和B樣條提出一種雙機器人系統的協調運動方案、避障算法、軌跡規劃方法以及優化方法。該方法可以規劃出一條避免碰撞干涉、執行時間較短、關節軌跡較為平滑、起始與終止的速度、加速度、加加速度可以任意配置的軌跡，并且降低了插值引起的誤差。采用此方法所得到的軌跡保證了裝配過程中主-從機器人驅動的穩定性、平滑性以及裝配的高效率性。此外，該方法可以推廣到多機器人協調裝配、協同定位等問題的軌跡規劃應用中，同時該方法有望集成到雙機器人離線仿真軟件中。