董方方， 喻 斌， 趙曉敏， 陳 珊

（1. 合肥工業(yè)大學 機械工程學院，合肥 230009；2. 安徽省智能數(shù)控技術及裝備工程實驗室，合肥 230009；3. 合肥工業(yè)大學 汽車與交通工程學院，合肥 230009）

引 言

移動機械臂的多機協(xié)作系統(tǒng)可廣泛應用于各種不同場合，如用于風電葉片、火箭殼體、高鐵車廂等大尺寸零部件表面的拋磨加工、焊接，以及低剛度、低穩(wěn)定性零件的搬運、裝配等工作.雙臂空間協(xié)作是多機協(xié)作中的典型案例，雙臂相比于單臂，具有靈活、高效、相對低成本、高可靠性等諸多優(yōu)點[1]，能夠更好地模仿人類手臂的工作情況.Yan等[2]針對雙機協(xié)作與單臂作業(yè)的差別與優(yōu)勢、協(xié)作機械臂的工作環(huán)境與資源沖突等諸多問題進行了具體的分析調研.對于需要大范圍移動工作的情況，移動機械臂相比于傳統(tǒng)的三坐標機械臂，占地面積更小，也更加靈活[3]，20世紀80年代興起的同步定位與建圖技術，使得移動機器人在陌生環(huán)境中的適應性得到了極大提升[4].

協(xié)作機械臂的建模與控制問題受到了眾多研究者的青睞，目前動力學建模常用的方法一般為Lagrange法[5]與Newton-Euler法[6]，此外還有基于Lagrange乘子法的Lie群方法[7]、Kane方法[8]等.Jorge等[9]提出了一種通過Lagrange法建立起單臂作業(yè)的動力學模型，再引入雙臂協(xié)作搬運時關節(jié)變量的耦合約束，建立了雙機協(xié)作搬運機械臂動力學模型的方法.Parra-Vega等[10]針對受約束協(xié)作多指抓取機器人，通過Lagrange法，建立了其摩擦模型與動力學模型.Flixeder等[11]針對協(xié)作搬運易形變材料的問題，分別針對易形變材料與協(xié)作機械臂兩部分進行了研究并建立了交互模型.章定國[12]在計算多桿機器人的動力學模型時采用將Lagrange方程與齊次變換矩陣相結合的方式，通過遞推策略求得了結果.郭益深等[13]結合Lagrange法與動量守恒理論，建立了雙臂空間機器人的模型.Reza等[14]應用多個力傳感器，提出了一種針對未知搬運物體的質心估測方法，將未知搬運物體固定在三個虛擬連桿上，并將連桿視作機械臂的一部分進行建模.

移動機械臂空間協(xié)作系統(tǒng)具有諸多優(yōu)勢，但由于其存在復雜的耦合關系，使得應用Lagrange法或Newton-Euler法直接進行動力學建模時的計算極為繁瑣，應用Lagrange乘子法雖然計算結果的形式相對簡潔，但建模思路抽象，且需要引入新的Lagrange乘子.Udwadia與Kalaba[15-16]提出了一種全新的觀點，稱為Udwadia-Kalaba（U-K）方法.他們將系統(tǒng)分割為若干子系統(tǒng)并建立動力學模型，通過U-K方程引入各子系統(tǒng)的固有幾何約束，無需引入輔助變量即可構建復雜系統(tǒng)整體動力學方程，從而達到簡化計算的目的.

已經(jīng)有許多研究者基于U-K方法進行了探索.韓江等[17]以平面2自由度冗余驅動并聯(lián)機器人作為研究對象，將系統(tǒng)分解為三個無約束開鏈子系統(tǒng)，建立無約束子系統(tǒng)模型后再引入支鏈末端位置約束，利用U-K方法求取約束力建立起其動力學方程.Huang等[18]針對并聯(lián)機械臂，應用U-K方法構建了其正逆動力學方程.韓江等[19]提出了一種基于U-K方法的協(xié)作機械臂軌跡跟蹤控制.劉佳等[20]和Zhen[21]針對雙機平面協(xié)作搬運問題，將系統(tǒng)從一條機械臂基座處斷開，形成虛擬5自由度機械臂進行建模，再利用U-K方法引入基座約束建立了動力學模型.

目前基于U-K方法，針對協(xié)作雙臂搬運系統(tǒng)建模問題，大多是基于平面動力學系統(tǒng)進行研究.本文結合Lagrange方法與U-K方法，建立了雙移動機械臂空間協(xié)作搬運的動力學模型.由于本文的研究對象中存在移動平臺，且平臺底面必須始終與地面平行，故針對本文的研究對象，在建模過程中若直接采用文獻[20-21]的方法，將機械臂末端連桿與旋轉關節(jié)視作斷開進行建模，雖然形成5自由度虛擬機械臂的方式同樣易于理解，但會導致無法引入斷開端基座電機轉角與斷開端連桿轉角的約束，造成約束缺失，且非線性耦合嚴重，最終表達式極其繁瑣.故本文提出了一種基于對稱思路的建模方法，該方法將負載中心處視作斷開，以將整體系統(tǒng)分割成兩個子系統(tǒng)，兩個子系統(tǒng)擁有高度對稱的動力學模型特征與約束關系，因此只需要利用Lagrange方法對其中一個子系統(tǒng)進行建模，即可得出兩個無約束子系統(tǒng)的動力學模型，再將固有幾何約束與期望軌跡以約束形式給出，即可通過U-K方程建立起整體模型.該方法的對稱建模思路，使得建立子系統(tǒng)動力學模型與約束方程時只需要考慮某一子系統(tǒng)即可得出整體方程，簡化了建模所需計算量，同時也避免了約束信息缺失的問題，且該方法探索了U-K方法在三維工作空間協(xié)作機械臂上的應用，將U-K方法針對雙臂協(xié)作機器人的應用推廣到了空間協(xié)作的情況.

1 應用U-K方法的一般步驟

U-K方法在穩(wěn)定無約束子系統(tǒng)的動力學方程的基礎上，向子系統(tǒng)施加約束力，子系統(tǒng)同時受到平衡力與約束力的作用，使其穩(wěn)定且始終滿足給定的約束條件.根據(jù)U-K理論的一般應用方法[22]，建立具有n個自由度的動力學系統(tǒng)模型的步驟如下：

（Ⅰ）建立無約束系統(tǒng)動力學方程：

式中，t為時間，M 為系統(tǒng)的 n×n維 慣性矩陣，q為廣義坐標，Q為施加在系統(tǒng)上的廣義力.

式中，A為 m×n維 矩陣，b為 m維 向量.

（Ⅲ）計算約束力，受約束系統(tǒng)動力學方程為

式中，Qc為約束力，上標“＋”表示矩陣的Moore-Penrose廣義逆矩陣.

對于一般的機械系統(tǒng)，現(xiàn)提出如下假設.

假設1對于任意 q∈Rn，總有 |M(q)|＞0[23].

假設2對于任意 q∈Rn，矩陣的秩 rank(A)≥1.

假設3對于任意初始條件 q∈Rn均滿足約束方程，且約束連續(xù).

2 雙移動機械臂空間協(xié)作搬運建模

單個移動機械臂由四自由度機械臂與移動平臺構成，移動平臺使用全向輪或Mecanum輪，可實現(xiàn)平面全向移動，機械臂擁有四個旋轉關節(jié)，如圖1所示.

圖 1 移動機械臂示意圖Fig. 1 The schematic of the mobile manipulator

建立如圖1、2所示的空間直角坐標系.以旋轉關節(jié) mr1與 mr2下表面圓心處建立坐標系 Op1Xp1Yp1Zp1與Op2Xp2Yp2Zp2， Zp1與 Zp2軸垂直于移動平臺上表面向上， Xp1與 Xp2軸平行于移動平臺上表面指向平臺前端（如圖2所示），并依據(jù)右手定則確定 Yp1與 Yp2軸.以初始位置處的機器人坐標系確定世界坐標系 OwXwYwZw.

圖2中部件 m3與 m6為機械臂末端負載，可簡化為連桿，并假想該連桿中心處分離進行建模，圖中變量Δxi和 Δyi分別表示平臺i 在 Xw和 Yw方向的位移，變量δi和θi分別表示旋轉關節(jié)i 和連桿i 的轉角，變量 Fxi和 Fyi分別表示平臺i 在 Xw和 Yw方向受到的驅動力，變量τi和ui分別表示旋轉關節(jié)i 和連桿i 的扭矩，物理量 mi，Ii，li分別表示部件i 的質量、轉動慣量和長度，Δz表示兩移動平臺高度差.

圖 2 雙移動機械臂空間協(xié)作系統(tǒng)示意圖： (a) 主視圖；(b) 俯視圖Fig. 2 The schematic of dual-system cooperative handling mobile manipulators: (a) the main view; (b) the vertical view

2.1 無約束子系統(tǒng)動力學方程的建立

在此動力學系統(tǒng)中，每個移動機械臂具有6個自由度，包含了與移動平臺相連接的關節(jié)轉動.由于移動平臺采用全向輪或Mecanum輪，在實際搬運過程中給定運動軌跡后，移動平臺的轉動并不會對搬運過程造成影響.為簡化計算，本文暫不考慮移動平臺的整體轉動.在不考慮約束條件下，該動力學系統(tǒng)共有12個自由度，需要12個狀態(tài)變量進行描述，選擇廣義坐標 q = [Δx1,Δy1,δ1,θ1,θ2,θ3,Δx2,Δy2,δ2,θ4,θ5,θ6]T，對該動力學系統(tǒng)施加外力 τ= [Fx1, Fy1,τ1, u1, u2, u3, Fx2, Fy2,τ2, u4, u5, u6]T.

與文獻[18-19]中末端關節(jié)與連桿處斷開方式相比，選擇負載中心處斷開的方式具有如下優(yōu)勢：第一，兩子系統(tǒng)動力學特性相同，故只需要針對某一子系統(tǒng)進行建模，即只需建立起無約束6自由度子系統(tǒng)模型，形式相對簡潔.若在本文研究對象中采用文獻[18-19]所用方法，雖然方法與形式上同樣易于理解，但需要建立起一個3自由度模型（移動平臺 + 旋轉關節(jié)）與一個8自由度模型（移動平臺 + 6自由度虛擬機械臂）.其中，8自由度模型計算量大且形式極其繁瑣.第二，若文獻[18-19]的方法應用在本文的研究對象上，會導致難以引入斷開端基座電機轉角與斷開端連桿轉角的約束，導致約束缺失.選擇負載中心處斷開則無需考慮該約束.

采用Lagrange方法，對圖2中左半子系統(tǒng)進行建模.由文獻[24]可知，對有n個自由度的一般系統(tǒng)應用Lagrange方程進行建模，可得到一般形式的動力學模型.其中慣性矩陣 M∈R12×12的計算方式如下：

首先計算各部分速度的Jacobi矩陣，可知

式中，νk和 ωk分別為機構k 質心處的線速度與角速度，和分別為矩陣和的第j 列.

計算得各部件質心處線速度，由式(6)計算可得Jacobi矩陣 Jv，左半子系統(tǒng)矩陣 JvL如下：

同理，可得到各部件質心處角速度，由式(7)計算可得Jacobi矩陣，左半子系統(tǒng)矩陣如下：

同理，可得矩陣G(q)∈R12的計算方式如下：

式中，Gi為矩陣 G 的第i 個元素，g為 廣義坐標下的重力加速度.

機械臂在轉動時會受到向心力與Coriolis力，可統(tǒng)一表示為，其中 C∈R12×12.向量是唯一確定的，但矩陣C 并不唯一.可以將矩陣規(guī)范為反對稱矩陣，以此確定矩陣C[25]，得到矩陣C的計算公式如下：

式中，Cij為矩陣C 的第 (i, j)個元素， Mij為矩陣 M 的第 (i, j)個元素.

由式(5)、(8)、(9)計算可得左半子系統(tǒng)矩陣 ML，GL，CL與右半子系統(tǒng)矩陣 MR， GR， CR. ML，GL，CL的具體計算結果見附錄.

至此，可得子系統(tǒng)形式的一般動力學方程：

將其變化為式(1)所示形式，可得

式中

2.2 約束方程的建立

假定機械臂進行水平搬運，假想斷開連桿實際為一整體，故假想末端 x,y,z 坐標應重合，且轉角 δ1,δ2及θ3,θ6應滿足物理結構約束條件.對位置所進行的約束，要求斷開處時刻保持位置相同，實際上已經(jīng)隱含了對速度和加速度的約束（將位置約束求一階導數(shù)即為對速度的約束，求二階導即為加速度約束），因此無需額外考慮對速度與加速度進行額外的約束.由于采用負載中心處進行斷開的方法，因此也無需考慮連桿轉角與關節(jié)轉角之間的約束關系，避免了約束信息缺失.由此，建立以下約束方程組：

顯然該約束為完整約束，將約束方程(12)對時間t 求二階導數(shù)，得到

式中

將約束方程二階導數(shù)寫成式(2)所示形式，可得

矩陣A與b的具體計算結果如下：

至此已經(jīng)得到矩陣 M, A,Q與 向量b，根據(jù)式(4)計算得約束力 Qc：

求得的約束力 Qc為系統(tǒng)受到約束(12)時產(chǎn)生的力，在無約束子系統(tǒng)的動力學方程(11)的右側添加求得的約束力，使得兩個子系統(tǒng)始終滿足約束(12)，從而得到協(xié)作系統(tǒng)動力學方程：

從以上建模過程中可以看出，采用U-K方法建模，不需要引入Lagrange乘子或者輔助變量，得到的是約束力的解析解，并且求解過程相對簡潔.

3 數(shù)值仿真分析

本節(jié)將通過仿真軟件進行數(shù)值仿真，以檢驗證模型的準確性.為了驗證模型的準確性，需要驗證兩假想斷開末端是否始終重合，δ1，δ1及θ3，θ6是否符合約束(12).為此，假想斷開連桿左端添加期望軌跡約束：

式中， xi,yi,zi為機械臂末端初始位置的坐標常量.

求二階導數(shù)得

表 1 系統(tǒng)動力學參數(shù)表Table 1 Dynamic parameters of system

此外，我們還需要知道系統(tǒng)的初始位置信息與初始速度和加速度信息，且初始狀態(tài)必須滿足約束條件(12)，給定如下初始位置、速度與加速度：取給定任意初值τ，圖3 ~ 6為仿真結果.

圖 3 斷開處軌跡示意圖Fig. 3 Trajectories at disconnection points

圖 4 關節(jié)角度約束示意圖Fig. 4 Joint angle constraints

圖 5 移動平臺位移示意圖Fig. 5 Displacements of mobile platforms

圖3中x， z方 向誤差量級為1 0-13，y方 向誤差量級為1 0-15，可視作負載斷開處運動時基本重合，x方 向無位移，y， z方向做周期往復運動，滿足固有幾何約束及給定運動約束.圖4中 θ3與 θ6誤差量級為1 0-13，δ1與 δ2方向誤差量級為1 0-15，可視為滿足固有幾何約束.移動平臺在x方 向保持靜止，在y 方向做周期往復運動，滿足給定運動約束，機械臂關節(jié)轉角在滿足約束的條件下做周期運動.仿真結果表明該方法構建的空間協(xié)作動力學模型準確可用.

圖 6 子系統(tǒng)轉角示意圖：(a) 左半子系統(tǒng)；(b)右半子系統(tǒng)Fig. 6 Rotation angles of subsystems: (a) the left subsystem; (b) the right subsystem

4 結 論

雙移動機械臂空間協(xié)作系統(tǒng)存在復雜的約束與非線性耦合，使得直接建立動力學模型極為困難，故本文中提出了一種基于U-K方程對系統(tǒng)建立動力學模型的方法.主要結論如下：

1) 將負載的中心處斷開，應用Lagrange方法建立了單個移動機械臂的動力學方程，減輕了系統(tǒng)的耦合現(xiàn)象，解決了引入約束方程時無法對末端電機轉角與末端連桿轉角加以限制的問題.

2) 給出了將雙移動機械臂空間協(xié)作的固有幾何約束及其二階導數(shù)，應用U-K方程建立了雙移動機械臂空間協(xié)作系統(tǒng)的動力學模型.該方法思路清晰，無需引入輔助變量，簡化了計算量.此外，現(xiàn)有針對雙臂協(xié)作問題的研究主要是基于平面協(xié)作進行的，而本文針對空間協(xié)作機械臂開展了應用U-K方法的探索.

3) 對建立的模型進行了數(shù)值仿真，對模型添加期望的軌跡約束，給定了符合約束方程的初始條件，仿真顯示模型運動軌跡與期望軌跡基本重合，證明了該方法建立動力學模型的可行性.由于該方法未考慮負載與機械臂之間的密度變化，且將連桿質心簡單視作連桿中心，因此存在一定的建模誤差.后續(xù)的研究可以把此類誤差統(tǒng)一整合至不確定性中，進一步基于該模型進行控制算法的探索.

附 錄