液壓并聯(lián)機(jī)器人力/位混合控制策略研究

陶 晗 曲智勇 叢大成

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

0 引言

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

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

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

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

1 系統(tǒng)模型

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

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

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

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

1.2 系統(tǒng)建模

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

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

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

(1)

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

q——鉸點(diǎn)位置向量

A——上鉸點(diǎn)的位置矩陣

B——下鉸點(diǎn)的位置矩陣

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

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

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

(2)

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

lm——并聯(lián)機(jī)構(gòu)支腿測(cè)量長(zhǎng)度

ln——第n次迭代計(jì)算出來的支腿長(zhǎng)度

J——雅可比變換矩陣

(3)

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

(4)

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

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

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

Fe——廣義接觸力向量

2 控制系統(tǒng)

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

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

(5)

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

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

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

PL=P1-n1P2

(6)

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

P1——液壓缸有桿腔壓力

P2——液壓缸兩腔的壓力

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

u3=V(JT)-1G

(7)

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

(8)

式中kP——比例項(xiàng)6×6正交增益矩陣

el——控制器輸入信號(hào)

u——控制器輸出信號(hào)

kI——積分項(xiàng)6×6正交增益矩陣

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

(9)

其中

式中Ps——油源壓力

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

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

(10)

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

(11)

當(dāng)平臺(tái)未發(fā)生碰撞時(shí)，將式(10)代入式(11)并簡(jiǎn)化可得

(12)

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

(13)

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

2.2 力/位混合控制

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

(14)

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

F——實(shí)際測(cè)量的作用力向量

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

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

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

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

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

(15)

式中eqd——相對(duì)位置偏差信號(hào)期望值

eq——相對(duì)位置偏差信號(hào)測(cè)量值

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

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

(16)

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

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

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

(17)

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

(18)

2.3 整體控制策略

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

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

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

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

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

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

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

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

3.1 動(dòng)態(tài)位置跟蹤試驗(yàn)

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

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

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

3.2 動(dòng)態(tài)對(duì)接試驗(yàn)

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

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

圖6 動(dòng)態(tài)對(duì)接試驗(yàn)結(jié)果Fig.6 Dynamic docking experiment

4 結(jié)論

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

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