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

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

引 言

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

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

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

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

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

1 應(yīng)用U-K方法的一般步驟

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

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

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

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

（Ⅲ）計(jì)算約束力，受約束系統(tǒng)動(dòng)力學(xué)方程為

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

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

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

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

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

2 雙移動(dòng)機(jī)械臂空間協(xié)作搬運(yùn)建模

單個(gè)移動(dòng)機(jī)械臂由四自由度機(jī)械臂與移動(dòng)平臺(tái)構(gòu)成，移動(dòng)平臺(tái)使用全向輪或Mecanum輪，可實(shí)現(xiàn)平面全向移動(dòng)，機(jī)械臂擁有四個(gè)旋轉(zhuǎn)關(guān)節(jié)，如圖1所示.

圖 1 移動(dòng)機(jī)械臂示意圖Fig. 1 The schematic of the mobile manipulator

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

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

圖 2 雙移動(dòng)機(jī)械臂空間協(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)動(dòng)力學(xué)方程的建立

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

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

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

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

式中，νk和 ωk分別為機(jī)構(gòu)k 質(zhì)心處的線速度與角速度，和分別為矩陣和的第j 列.

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

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

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

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

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

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

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

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

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

式中

2.2 約束方程的建立

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

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

式中

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

矩陣A與b的具體計(jì)算結(jié)果如下：

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

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

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

3 數(shù)值仿真分析

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

式中， xi,yi,zi為機(jī)械臂末端初始位置的坐標(biāo)常量.

求二階導(dǎo)數(shù)得

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

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

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

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

圖 5 移動(dòng)平臺(tái)位移示意圖Fig. 5 Displacements of mobile platforms

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

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

4 結(jié) 論

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

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

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

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

附 錄