陶 晗 曲智勇 叢大成

(哈爾濱工業(yè)大學(xué)機電工程學(xué)院， 哈爾濱 150001)

0 引言

由于具有承載能力強、響應(yīng)速度快、高剛度、高精確度等特點，并聯(lián)機構(gòu)已經(jīng)在各個行業(yè)得到了廣泛的應(yīng)用[1-2]。尤其是六自由度Stewart平臺，在外科手術(shù)和裝配、剖光等需要和環(huán)境密切接觸的領(lǐng)域中，并聯(lián)機構(gòu)同樣因其高精度的特點得到了廣泛的應(yīng)用[3]。

目前，六自由度的各種運動控制方法已經(jīng)得到了廣泛的研究，其技術(shù)相對比較成熟。然而由于工作環(huán)境的不確定性，與并聯(lián)機器人的運動控制相比，其需要與環(huán)境接觸的力控制策略更為復(fù)雜[3-5]。為了能夠?qū)崿F(xiàn)更為復(fù)雜的任務(wù)同時提高并聯(lián)機構(gòu)的可操控性，基于多傳感器的控制方式被開發(fā)出來，其系統(tǒng)的工作效率效率、魯棒性和柔順性都得到了相應(yīng)的提升[6]。在諸多位置測量方式中，視覺傳感器因其可以在較大范圍空間中進行非接觸位置測量而得到廣泛的應(yīng)用[7]。通過引入機器視覺，控制系統(tǒng)對執(zhí)行器和交互目標(biāo)在空間的位置精確度的要求更低，能夠更好地完成與目標(biāo)的交互任務(wù)[3]。

將視覺傳感器和力傳感器相結(jié)合可以克服其自身各自的局限性，通過視覺傳感器對目標(biāo)進行非接觸的測量并對整個工作環(huán)境進行建模，使用力傳感器檢測與環(huán)境互動的作用力以保證交互任務(wù)的安全性。目前已經(jīng)有學(xué)者對這種基于視覺的力/位混合控制進行研究[5-7]。

液壓并聯(lián)機器人由于其液壓系統(tǒng)自身高度非線性的特點，基于自身動力學(xué)建模的力位混合控制常常難以實現(xiàn)[8-11]。為了使六自由度液壓并聯(lián)機構(gòu)更好地實現(xiàn)與目標(biāo)互動對接，文本提出使用視覺和力傳感器融合的力/位混合控制策略，并對提出的混合控制進行一系列對接試驗驗證，以證明這種控制方式的有效性。

1 系統(tǒng)模型

1.1 液壓并聯(lián)機構(gòu)與目標(biāo)工裝

整個系統(tǒng)的構(gòu)成如圖1所示，為一水平放置的液壓驅(qū)動六自由度并聯(lián)機構(gòu)，其下平臺固定在地基上，上平臺上搭載著對接裝置，可以在工作空間內(nèi)自由移動以捕捉目標(biāo)工裝[10]。平臺由6個液壓缸進行驅(qū)動，每個液壓缸通過虎克鉸與上下平臺進行連接。其中，每個液壓驅(qū)動器由1個非對稱伺服閥和非對稱液壓缸構(gòu)成。在上平臺和對接裝置之間設(shè)有一個平板式六自由度力傳感器以測量并聯(lián)平臺與環(huán)境之間力、力矩作用。

圖1 液壓并聯(lián)機構(gòu)及目標(biāo)工裝Fig.1 Hydraulic manipulator and target object1.地基 2.液壓并聯(lián)機構(gòu) 3.力傳感器 4.末端執(zhí)行器 5.滑動軌道 6.對接裝置 7.激光靶盤 8.激光測距儀 9.攝像頭

考慮到固定在滑動平臺上的目標(biāo)工裝質(zhì)量較大且需要在較大的范圍內(nèi)進行擺動，試驗中設(shè)置x、y、z方向3條互相垂直的滑動軌道，并通過步進電機進行驅(qū)動，以保證目標(biāo)工裝可以連續(xù)平穩(wěn)地滑動。為了更好地與目標(biāo)工裝進行對接，在平臺對接機構(gòu)上安裝了2個圓柱形導(dǎo)向銷并在目標(biāo)工裝上對應(yīng)設(shè)置了2個導(dǎo)向孔，當(dāng)二者完全對接后，通過氣缸進行鎖緊。為了得到更為精確的并聯(lián)平臺與目標(biāo)工裝之間的相對距離，本方案通過CCD攝像機捕捉目標(biāo)工裝上的二維靶標(biāo)，以計算二者之間的相對位姿。為了彌補這種單目視覺對相對距離計算不準(zhǔn)確的問題，使用了激光測距儀對所測得距離進行修正。

1.2 系統(tǒng)建模

如圖1所示，體坐標(biāo)系Opxpypzp固定在動平臺上，同時靜坐標(biāo)系Obxbybzb固定在基座上。在初始狀態(tài)下，兩個坐標(biāo)系的原點重合。

通過運動學(xué)反解[12]計算并聯(lián)機構(gòu)液壓驅(qū)動器液壓缸的缸長，缸長計算表達式為

l=‖R(qo)A-B+qp‖

(1)

式中l(wèi)——上下鉸點間距離

q——鉸點位置向量

A——上鉸點的位置矩陣

B——下鉸點的位置矩陣

R——從體坐標(biāo)系到慣性坐標(biāo)系之間的轉(zhuǎn)換矩陣

通過運動學(xué)正解[19]，用并聯(lián)機構(gòu)驅(qū)動器長度l計算出上平臺位置坐標(biāo)q。使用牛頓歐拉法，計算公式為

qn+1=qn+J(qn)-1(lm-ln)

(2)

式中n——迭代的次數(shù)

lm——并聯(lián)機構(gòu)支腿測量長度

ln——第n次迭代計算出來的支腿長度

J——雅可比變換矩陣

(3)

并聯(lián)平臺的動力學(xué)模型可以表示為

(4)

式中M——質(zhì)量矩陣G——重力矩陣

C——摩擦力和科氏力矩陣

f——驅(qū)動器力向量

Fe——廣義接觸力向量

2 控制系統(tǒng)

2.1 控制器內(nèi)環(huán)

控制系統(tǒng)位置內(nèi)環(huán)基于鉸點空間坐標(biāo)系建立，通過運動學(xué)反解計算出平臺特定位姿的液壓缸支腿長度。因此，通過運動學(xué)反解可以計算每個驅(qū)動器所需要的長度，以控制平臺實現(xiàn)指定位姿。系統(tǒng)通過位移傳感器采集液壓缸的伸出長度來實現(xiàn)位置閉環(huán)，并采用動壓反饋來進一步提升系統(tǒng)的動態(tài)特性[13]。當(dāng)液壓并聯(lián)機構(gòu)在某一方向跟蹤正弦信號時，往返方向負(fù)載特性的非對稱性將降低跟蹤精度。而且，在這種側(cè)向放置的并聯(lián)機構(gòu)中重力作為外界干擾將進一步增大系統(tǒng)的穩(wěn)態(tài)誤差，因此在控制器系統(tǒng)設(shè)計中采用了動態(tài)重力補償(DGC)以期提高系統(tǒng)整體性能[12]。另外，液壓伺服閥的負(fù)載敏感性也會影響整個閉環(huán)系統(tǒng)的性能，本文采用了非線性補償環(huán)節(jié)以抵消其對性能產(chǎn)生的影響。整個系統(tǒng)的內(nèi)環(huán)位置控制框圖如圖2所示[13-15]。

圖2 位置內(nèi)環(huán)控制策略Fig.2 Block diagram of inner loop controller

在控制內(nèi)環(huán)中動壓反饋系數(shù)u2[16]為

(5)

式中Tp——時間常數(shù)

kfp——6×6正交增益矩陣

PL為負(fù)載壓力矩陣，其定義如下

PL=P1-n1P2

(6)

式中n1——液壓缸有桿腔和無桿腔有效面積比例

P1——液壓缸有桿腔壓力

P2——液壓缸兩腔的壓力

如圖2所示，根據(jù)由運動學(xué)正解實時解算出來的平臺當(dāng)前位置，可以解算出液壓平臺的重力項。矩陣J將重力項G從笛卡爾空間坐標(biāo)系轉(zhuǎn)換到鉸點坐標(biāo)系[8-9]。如圖2所示，通過轉(zhuǎn)換矩陣V將鉸點空間上的作用力轉(zhuǎn)換到液壓缸伺服閥上的信號。其控制方法可以表示為

u3=V(JT)-1G

(7)

與PI控制器相結(jié)合，整個控制方案表示為

(8)

式中kP——比例項6×6正交增益矩陣

el——控制器輸入信號

u——控制器輸出信號

kI——積分項6×6正交增益矩陣

為了降低伺服閥的負(fù)載敏感性，非線性補償模塊設(shè)計為

(9)

其中

式中Ps——油源壓力

PLi——第i個液壓缸的負(fù)載壓降

這樣，最終整個控制系統(tǒng)的內(nèi)環(huán)控制表達為

(10)

聯(lián)立式(5)、(10)，并聯(lián)系統(tǒng)控制表達式為

(11)

當(dāng)平臺未發(fā)生碰撞時，將式(10)代入式(11)并簡化可得

(12)

當(dāng)系統(tǒng)達到穩(wěn)態(tài)時，平臺的速度、加速度和壓力變化等于零，式(12)可簡化為

(13)

如上所述，文中提出的控制系統(tǒng)達到穩(wěn)態(tài)時其穩(wěn)態(tài)偏差將逐漸接近于零。

2.2 力/位混合控制

通過6×6正交分解矩陣，將整個工作空間坐標(biāo)系分解為位置控制空間Sq和力控制空間 (I-S)q[3]。在力控制空間Sq，控制系統(tǒng)表達式為

(14)

式中Fd——期望的作用力向量

F——實際測量的作用力向量

kFP——6×6正交比例增益矩陣

kFI——6×6正交積分增益矩陣

qd——6×1期望位置向量

qr——6×1參考位置向量

通過CCD攝像頭和激光測距器所測量出來的位置信息是并聯(lián)機構(gòu)和目標(biāo)工裝的相對位置偏差信號，所以參考位置信號qr可表示為

(15)

式中eqd——相對位置偏差信號期望值

eq——相對位置偏差信號測量值

kPI——6×6的正交增益矩陣

將式(15)代入式(14)中，力控制策略表達式可表達為

(16)

事實上，力控制系統(tǒng)的建立是基于由HOGAN[4]提出的阻抗控制策略。這種控制方式將并聯(lián)平臺的位置和力保持一個動態(tài)關(guān)系，可根據(jù)實際情況調(diào)整整個系統(tǒng)的阻抗來滿足不同的工況需要[16]。

對于位置控制空間(I-S)q，其控制策略表達為

(I-S)qd=(I-S)qr

(17)

將式(16)和式(17)聯(lián)立，整個系統(tǒng)的力/位混合控制策略可表達為

(18)

2.3 整體控制策略

整個混合控制策略流程圖如圖3所示。由圖3可以看到，控制系統(tǒng)由內(nèi)環(huán)外環(huán)兩層構(gòu)成，其中內(nèi)環(huán)實現(xiàn)500 Hz的高頻位置和速度跟蹤功能，外環(huán)以相對慢的速度(100 Hz)計算出相對平穩(wěn)地運動信號以修正與環(huán)境接觸相對作用力[20]。整個系統(tǒng)的工作流程包括對接、跟隨并鎖緊和解鎖并返回。在實現(xiàn)整個工作流程中，并聯(lián)機構(gòu)需要根據(jù)目前的相對位姿和相互作用力的信息來修正路徑。圖3為系統(tǒng)的定時器和路徑規(guī)劃模塊。其路徑規(guī)劃模塊基于Matlab的狀態(tài)機編制，通過采集目前的平臺和目標(biāo)工裝的相對位置和作用力決定平臺的移動速度和方向。

圖3 控制系統(tǒng)整體框架Fig.3 Hybrid control system of manipulator

為保證并聯(lián)機構(gòu)自動對接過程的安全性、可靠性，要求機械部分以光滑連續(xù)的軌跡完成對目標(biāo)靶盤的伺服跟蹤過程，采用五次多項式插值可得到滿足約束要求的軌跡方程[17-18]。

在液壓并聯(lián)機構(gòu)實際運行過程中，可能會在并聯(lián)機構(gòu)與目標(biāo)工裝之間出現(xiàn)障礙或者目標(biāo)工裝運動幅度、速度過大超出能夠正常對接交互范圍時，路徑規(guī)劃模塊會暫?；驕p緩機器人z方向與目標(biāo)接近運動，以防止對并聯(lián)機構(gòu)或目標(biāo)工裝造成破壞。

3 試驗與結(jié)果分析

圖4 試驗系統(tǒng)結(jié)構(gòu)Fig.4 Experimental setup

如圖4所示，整個試驗系統(tǒng)包括液壓并聯(lián)機構(gòu)和目標(biāo)模擬運動裝置兩部分。目標(biāo)模擬運動裝置模擬目標(biāo)在真實工況下的各種運動，包括管理計算機PC2和目標(biāo)工裝的驅(qū)動系統(tǒng)。液壓并聯(lián)機構(gòu)的控制部分則包括任務(wù)管理PC1和運動控制器、油源管理器。2個運動管理計算機PC1和PC2都是基于NI PXI-8196嵌入式控制系統(tǒng)，其通過CAN總線互相分享信息。其機器視覺系統(tǒng)工作頻率100 Hz，硬件包括一個精度達1 mm的ZYT-0060型激光測距器和分辨率658像素×492像素的Mako G-032型高性能相機。接觸力傳感器采用BERTEC六維平板式力傳感器，其在y、z方向最大量程10 000 N，x方向最大量程則達20 000 N。

為了驗證所提出的控制策略的有效性，首先進行了平臺的動態(tài)位姿跟蹤試驗，試驗中通過使平臺在初始位置跟蹤目標(biāo)工裝y、z方向的運動，以驗證算法的基于視覺的位置跟蹤性能。然后，試驗系統(tǒng)進行了完整對接試驗以驗證提出的基于視覺的力/位混合控制策略。

3.1 動態(tài)位置跟蹤試驗

動態(tài)位姿跟蹤試驗主要用來驗證平臺對目標(biāo)工裝移動的跟蹤性能，同時也為下一步動態(tài)對接試驗做準(zhǔn)備。為了盡量消除慣性力對試驗的影響，目標(biāo)工裝在步進電機的驅(qū)動下做近似三角波運動。目標(biāo)工裝運動速度分別為y向±100 mm/s和z向±80 mm/s，其運動幅值設(shè)定為±100 mm。在試驗開始時，平臺固定在其初始零位，在控制系統(tǒng)中的相對位置為x=0，y=0，z=120 mm。試驗結(jié)果如圖5所示，具體分別為y和z方向的位置偏差、平臺受力和平臺位置。

圖5 位置跟蹤試驗結(jié)果Fig.5 Dynamic tracking experiment

整體看來，試驗中y、z方向的動態(tài)位姿跟蹤誤差均小于10 mm，同時z方向的跟蹤誤差要較大一些而且存在著一定的噪聲，其原因是跟蹤過程中受到平臺自身重力的影響。y和z方向的力偏差較小但是仍存在一定的噪聲，從圖5c、5d可以看出，大概有±70 N的偏差，這是因為控制系統(tǒng)中并未考慮慣性力對系統(tǒng)的影響[8]。位置控制的給定信號和運動學(xué)正解計算的平臺實際位置信號對比如圖5e、5f所示，可以看出，液壓并聯(lián)平臺的位置控制精度較高，這也證實了2.1節(jié)中所提出的位置控制內(nèi)環(huán)的算法有效性。

3.2 動態(tài)對接試驗

整個動態(tài)對接過程包含7個階段，每個階段的具體起點和終點在圖6中標(biāo)出：初始重置、回退、非接觸對接、接觸對接、鎖緊并跟蹤、解鎖并回退以及最后的終止階段。目標(biāo)工裝通過滑軌進行x、y、z3個方向的混合運動，其x、y軸方向速度50 mm/s，z軸方向速度40 mm/s，兩個方向的運動速度幅值均為±100 mm。在初始階段，并聯(lián)平臺鎖定在初始零位，目標(biāo)工裝固定在慣性坐標(biāo)系下x=133 mm、y=-26 mm、z=-93 mm位置。為了進一步提高對接速度，x方向的進給速度由y、z方向的位置偏差決定。當(dāng)x方向距離較大或y、z方向的位置偏差小于10 mm時，平臺將沿著x方向接近目標(biāo)工裝。當(dāng)平臺末端與目標(biāo)工裝距離小于144 mm時，y、z方向的位置誤差允許范圍降到7 mm，這是為了保證平臺末端的導(dǎo)向銷可以準(zhǔn)確平穩(wěn)地插入目標(biāo)工裝上的導(dǎo)向孔。

如圖6所示，在回退階段，y和z方向的相對位置誤差快速進入額定范圍之內(nèi)，x方向在15 s時開始移動。其中，y、z方向的位置和作用力較為接近，故只用y方向數(shù)據(jù)表示。在第51秒之前，并聯(lián)平臺和目標(biāo)工裝之間并未發(fā)生接觸故測量到的接觸力很小。發(fā)生接觸之后，通過文中所提出的基于機器視覺的力/位混合控制策略，其接觸力可以控制在300 N以內(nèi)。從圖6中可以清楚地看出，從第15秒到67秒之間的對接過程，路徑生成模塊不斷地發(fā)出x方向的前進指令以保證整個對接過程平穩(wěn)地進行。試驗結(jié)果證明了文中所提出的控制算法的有效性。

圖6 動態(tài)對接試驗結(jié)果Fig.6 Dynamic docking experiment

4 結(jié)論

(1)提出了一種用于實現(xiàn)液壓并聯(lián)平臺與環(huán)境對接交互任務(wù)的基于機器視覺的力/位混合控制策略。控制系統(tǒng)由內(nèi)外兩層閉環(huán)構(gòu)成，在內(nèi)環(huán)中，由安裝在液壓驅(qū)動器上的油壓傳感器和位置傳感器提供閉環(huán)反饋信息，并采用了動壓反饋和非線性補償來進一步提高系統(tǒng)的動態(tài)性能。在控制系統(tǒng)外環(huán)，通過CCD攝像機和激光測距器來提供全局相對位置并使用六維力傳感器測量在對接過程中的接觸力。系統(tǒng)控制外環(huán)將采集到的位置信息和力的信息綜合以決定平臺的運動軌跡，同時通過控制系統(tǒng)內(nèi)環(huán)來實現(xiàn)對目標(biāo)工裝的精確跟蹤。

(2)建立了完整的試驗系統(tǒng)模型并對文中提出的控制算法進行了若干驗證試驗。在動態(tài)位姿跟蹤試驗中，目標(biāo)工裝通過進行三角波運動以期減弱并聯(lián)機構(gòu)慣性力對控制系統(tǒng)穩(wěn)定性的影響。在目標(biāo)以高速運動的狀態(tài)下，x、y方向跟蹤的相對位置誤差均不超過10 mm。之后進行了快速的動態(tài)對接試驗，試驗中實現(xiàn)了對目標(biāo)快速的跟蹤和穩(wěn)定的對接交互，且整個對接過程中相對作用力均不超過300 N，能夠很好地完成裝配等環(huán)境交互任務(wù)，滿足安全性要求。