徐春博 趙丁選,2 孔維天 鄒少元 倪 濤 舒禮志

(1.吉林大學(xué)機(jī)械與航空航天工程學(xué)院， 長(zhǎng)春 130022； 2.燕山大學(xué)機(jī)械工程學(xué)院， 秦皇島 066004)

0 引言

在海洋浪涌的影響下，船舶產(chǎn)生了橫傾、縱傾和升沉等運(yùn)動(dòng)，海洋物流吊裝設(shè)備的安全性、可靠性和工作效率受到了嚴(yán)重的影響。目前主要通過(guò)海浪補(bǔ)償、吊重減擺和主動(dòng)軌跡規(guī)劃等方法實(shí)現(xiàn)吊運(yùn)的智能控制。

對(duì)于岸對(duì)船或船對(duì)岸吊裝，雖然港口或海岸是靜止的，但在淺岸海況較差時(shí)，仍需知道在浪涌作用下船舶相對(duì)港口地平面的升沉偏移，以實(shí)現(xiàn)補(bǔ)償以及平穩(wěn)的吊裝作業(yè)[1]。對(duì)于船對(duì)船的情況，王生海[2]提出一種基于波浪趨勢(shì)判斷的主動(dòng)式波浪補(bǔ)償控制系統(tǒng)，通過(guò)氣介式聲學(xué)波浪儀和運(yùn)動(dòng)參考單元(Motion reference units，MRU)等傳感器網(wǎng)絡(luò)感受波浪起伏狀況、船舶姿態(tài)等運(yùn)動(dòng)，由變參數(shù)比例積分微分算法(Proportion integral differential，PID)控制液壓執(zhí)行機(jī)構(gòu)進(jìn)行主動(dòng)補(bǔ)償。尹海兵等[3]用MRU和位移傳感器分別計(jì)算要求補(bǔ)償位移和實(shí)際位移，并進(jìn)行比較，進(jìn)而通過(guò)調(diào)節(jié)液壓缸位移補(bǔ)償波浪對(duì)船體的影響。GODHAVN[4]采用全球定位系統(tǒng)(Global positioning system，GPS) 實(shí)時(shí)動(dòng)態(tài)(Real time kinematic，RTK)和MRU估計(jì)船舶的運(yùn)動(dòng)姿態(tài)，并成功應(yīng)用在海浪補(bǔ)償系統(tǒng)中。徐小軍等[5]設(shè)計(jì)了一種基于數(shù)字信號(hào)處理(Digital signal processing，DSP)的船用起重機(jī)控制系統(tǒng)，具有良好的工作穩(wěn)定性?？婗蝃6]將偽微分控制策略和神經(jīng)網(wǎng)絡(luò)進(jìn)行結(jié)合，提出了一種新的控制策略。SAGATUN[7]針對(duì)船用吊裝系統(tǒng)提出了前饋補(bǔ)償?shù)目刂品椒?，?shí)現(xiàn)了主動(dòng)補(bǔ)償控制技術(shù)。何平[8]對(duì)前饋補(bǔ)償和反饋校正結(jié)合的補(bǔ)償系統(tǒng)進(jìn)行了研究。馮林等[9]采用基于模糊自適應(yīng)的PID控制策略對(duì)主動(dòng)升沉補(bǔ)償系統(tǒng)進(jìn)行研究。

上述研究大多采用MRU對(duì)補(bǔ)償量進(jìn)行測(cè)量，其價(jià)格昂貴，具有一定的時(shí)滯性，并且此種方式在作業(yè)過(guò)程中需要人工輸入貨物位置信息，應(yīng)用成本較高。本文將計(jì)算機(jī)視覺(jué)引入海浪補(bǔ)償問(wèn)題中，提出一種基于視覺(jué)的卷?yè)P(yáng)隨動(dòng)控制系統(tǒng)，并嘗試將運(yùn)動(dòng)補(bǔ)償中保持相對(duì)靜止的問(wèn)題轉(zhuǎn)換為運(yùn)動(dòng)跟隨問(wèn)題進(jìn)行研究。該系統(tǒng)依托于實(shí)驗(yàn)室搭建的海洋物流裝備智能控制模擬試驗(yàn)平臺(tái)，采用機(jī)器視覺(jué)的方式，在手眼關(guān)系的基礎(chǔ)上，實(shí)現(xiàn)對(duì)目標(biāo)位置的定位與跟蹤，進(jìn)而控制吊裝機(jī)械臂卷?yè)P(yáng)升降，實(shí)現(xiàn)隨動(dòng)控制。

1 系統(tǒng)組成

圖1為卷?yè)P(yáng)隨動(dòng)控制系統(tǒng)，主要依托于搭建的綜合試驗(yàn)平臺(tái)，其主要包括運(yùn)動(dòng)模擬平臺(tái)、吊裝機(jī)械臂、視覺(jué)相機(jī)及標(biāo)識(shí)、卷?yè)P(yáng)機(jī)構(gòu)等。運(yùn)動(dòng)模擬平臺(tái)可根據(jù)海浪譜模型生成的波浪曲線(xiàn)模擬海浪運(yùn)動(dòng)對(duì)船舶造成的橫搖、縱搖、垂蕩等影響；卷?yè)P(yáng)機(jī)構(gòu)安置于吊裝機(jī)械臂的小臂末端，機(jī)械臂和卷?yè)P(yáng)整體模擬起重設(shè)備，實(shí)現(xiàn)折臂式起重機(jī)的回轉(zhuǎn)、起吊、前伸等運(yùn)動(dòng)的自動(dòng)控制，使末端卷?yè)P(yáng)到達(dá)指定的吊裝點(diǎn)上方。系統(tǒng)的控制流程框圖如圖2所示。

圖2 基于視覺(jué)的吊裝機(jī)械臂卷?yè)P(yáng)隨動(dòng)控制框圖Fig.2 Block diagram of vision-based hoisting robot follow-up control system

如圖2所示，控制系統(tǒng)主要分為視覺(jué)模塊和運(yùn)動(dòng)控制模塊。視覺(jué)模塊在組合標(biāo)識(shí)知識(shí)庫(kù)標(biāo)識(shí)、相機(jī)模型、卷?yè)P(yáng)執(zhí)行機(jī)構(gòu)和卷筒之間的手眼關(guān)系等前提信息基礎(chǔ)下，實(shí)現(xiàn)對(duì)組合標(biāo)識(shí)的檢測(cè)和定位，并且通過(guò)機(jī)械臂運(yùn)動(dòng)學(xué)使卷?yè)P(yáng)處于正常的工作位置。在視覺(jué)系統(tǒng)檢測(cè)到標(biāo)識(shí)后，首先采用四元數(shù)法以及平均權(quán)值向量法實(shí)現(xiàn)子標(biāo)識(shí)的位姿融合，其次進(jìn)行坐標(biāo)變換，得到了卷?yè)P(yáng)執(zhí)行機(jī)構(gòu)相對(duì)于標(biāo)識(shí)目標(biāo)對(duì)象的相對(duì)位姿，最后據(jù)此獲取卷?yè)P(yáng)執(zhí)行機(jī)構(gòu)相對(duì)目標(biāo)對(duì)象的高度。運(yùn)動(dòng)控制模塊通過(guò)卷?yè)P(yáng)的運(yùn)動(dòng)雅可比函數(shù)將運(yùn)動(dòng)控制量從笛卡爾空間變換到關(guān)節(jié)空間中，采用速度環(huán)反饋控制的方式，實(shí)現(xiàn)對(duì)關(guān)節(jié)控制量的連續(xù)控制。

2 運(yùn)動(dòng)學(xué)分析及手眼標(biāo)定

2.1 機(jī)器人正運(yùn)動(dòng)學(xué)

采用Kuka R16六自由度機(jī)械臂模擬折臂式起重機(jī)結(jié)構(gòu)。該機(jī)器人前三關(guān)節(jié)中，大臂模擬折臂式起重機(jī)的主臂，小臂模擬折臂式起重機(jī)的輔助臂，采用文獻(xiàn)[10]的約定對(duì)吊裝機(jī)械臂的運(yùn)動(dòng)學(xué)進(jìn)行建模，如圖3所示。其中，吊裝機(jī)械臂的末端為卷?yè)P(yáng)系統(tǒng)在機(jī)械臂小臂上的固定點(diǎn)，其坐標(biāo)系為{O4}，卷?yè)P(yáng)系統(tǒng)即末端執(zhí)行器的坐標(biāo)系為{O5}，這兩個(gè)坐標(biāo)系均為相對(duì)前一個(gè)坐標(biāo)系固定的位姿，坐標(biāo)系{O5}不隨卷筒的轉(zhuǎn)動(dòng)而變化。

圖3 吊裝機(jī)器人及卷?yè)P(yáng)幾何坐標(biāo)Fig.3 Hoisting robot and hoist geometric cooridnates

為方便從關(guān)節(jié)角3到末端執(zhí)行器坐標(biāo)系的建立

與坐標(biāo)變換的推導(dǎo)，將末端和末端執(zhí)行器也按照上述關(guān)節(jié)參數(shù)的定義統(tǒng)一到D-H表，其中關(guān)節(jié)3到末端坐標(biāo)系{O4}之間的距離作為桿件4，末端{(lán)O4}到末端執(zhí)行器{O5}的距離作為桿件5，根據(jù)圖3建立的D-H參數(shù)如表1所示。

表1 機(jī)器人D-H參數(shù)Tab.1 D-H parameters of robots

2.2 機(jī)器人逆運(yùn)動(dòng)學(xué)

根據(jù)文獻(xiàn)[11]的矩陣逆乘解析法求逆解。由正運(yùn)動(dòng)學(xué)已知末端執(zhí)行器相對(duì)基座總變換矩陣為

0T5=0T11T22T33T44T5

(1)

(0T1)-1、(1T2)-1、(2T3)-1依次左乘式(1)有

(0T1)-10T5=1T5=1T22T33T44T5

(2)

(1T2)-1(0T1)-10T5=1T5=2T33T44T5

(3)

(2T3)-1(1T2)-1(0T1)-10T5=1T5=3T44T5

(4)

由式(2)可得

式中s表示sin，c表示cos。

通過(guò)整理得到

令

m=nxny-oxoy-axay

解得

同理，由式(3)中左右兩邊3行1列和3行4列相等可解得

同理，由式(4)中3行4列元素為0可解得

2.3 手眼系統(tǒng)標(biāo)定

對(duì)機(jī)械臂進(jìn)行兩次旋轉(zhuǎn)軸非平行的位姿變換可得到相機(jī)相對(duì)參考物的坐標(biāo)變換oHc,1、oHc,2，機(jī)械臂末端執(zhí)行器相對(duì)基座的坐標(biāo)變換bHt,1、bHt,2，可構(gòu)建具有封閉環(huán)的坐標(biāo)轉(zhuǎn)換

(bHt,2)-1bHt,1X=X(oHc,2)-1oHc,1

(5)

其中

進(jìn)行多次測(cè)量以消除測(cè)量噪聲帶來(lái)的誤差，并進(jìn)行整體迭代優(yōu)化求解

AkX=XBk

(6)

其中

Ak=t,kAt,k-1=(bHt,k)-1bHt,k-1

Bk=c,kBc,k-1=(oHc,k)-1oHc,k-1

用Park-Martin算法[12]對(duì)X分步求解

(7)

(8)

其在李群中的對(duì)數(shù)映射為

(9)

其中

其中θ=arccos((Tr(R)-1)/2)。令lgAk=[αk]、lgBk=[βk]，即

(10)

由式(10)轉(zhuǎn)換為最小二乘擬合問(wèn)題，即

(11)

得到RX的最優(yōu)解為

(12)

其中

最后求得X的平移向量為

tX=(CTC)-1CTD

(13)

其中

通過(guò)控制吊裝機(jī)械臂在不同位置共采集16幅圖像，如圖4所示。

圖4 手眼標(biāo)定采集圖像Fig.4 Hand-eye calibration image acquisition

最終得到攝像頭與卷?yè)P(yáng)卷筒之間的手眼關(guān)系坐標(biāo)變換矩陣為

3 基于視覺(jué)標(biāo)識(shí)的目標(biāo)定位跟蹤

3.1 標(biāo)識(shí)的識(shí)別與定位

標(biāo)識(shí)識(shí)別主要包括圖像預(yù)處理與解碼。本小節(jié)以AprilTags標(biāo)識(shí)為例，對(duì)其進(jìn)行檢測(cè)識(shí)別算法處理，處理流程如圖5所示。

圖5 復(fù)雜環(huán)境下標(biāo)識(shí)檢測(cè)識(shí)別流程圖Fig.5 Identification detection flow chart in complex environment

首先對(duì)采集的圖像進(jìn)行灰度處理，將圖像特征分割為特征相同區(qū)域和不同區(qū)域，并對(duì)灰度圖進(jìn)行二值化處理，加大圖像對(duì)比度，過(guò)濾對(duì)比度不足的區(qū)域。接下來(lái)基于聯(lián)合查找聚類(lèi)算法，采用Jordan曲線(xiàn)定理查找每個(gè)像素塊的近鄰域具有相同明暗的部分[13]，并擬合成具有封閉輪廓的連通域，結(jié)合低通濾波以增加聚類(lèi)分割效率，結(jié)果如圖5d、5e所示。再對(duì)封閉輪廓進(jìn)行四邊形線(xiàn)段擬合，以少量角點(diǎn)作為節(jié)點(diǎn)，通過(guò)主成分分析(Principal component analysis，PCA)對(duì)兩相鄰節(jié)點(diǎn)之間邊界輪廓點(diǎn)進(jìn)行線(xiàn)性擬合，最佳擬合線(xiàn)段為第一主成分的特征向量，效果如圖5f所示。解碼之前按照預(yù)先設(shè)定的標(biāo)識(shí)種類(lèi)，對(duì)提取的輪廓透視投影為正方形圖像，劃分具有相同單元格規(guī)格的區(qū)域，從左至右分層提取標(biāo)識(shí)的黑色邊框內(nèi)部采樣點(diǎn)的黑白像素信息作為該處比特位，效果如圖5g所示。

基于Hamming距離對(duì)Apriltags標(biāo)識(shí)進(jìn)行解碼識(shí)別，將預(yù)定義的Hamming編碼以哈希表形式生成并對(duì)編碼信息序列進(jìn)行校驗(yàn)，最終獲得具有唯一編碼的識(shí)別號(hào)，檢測(cè)結(jié)果如圖5h所示。

為了提高定位精度，對(duì)原圖四邊形區(qū)域邊緣梯度進(jìn)行提取，沿著垂直邊緣法線(xiàn)方向朝標(biāo)識(shí)內(nèi)查找梯度值最大的位置排除噪點(diǎn)。以邊緣梯度為權(quán)重，對(duì)法線(xiàn)方向的點(diǎn)進(jìn)行加權(quán)平均，對(duì)同一邊的加權(quán)平均點(diǎn)做線(xiàn)段擬合，獲得標(biāo)識(shí)細(xì)化的邊緣，識(shí)別效果如圖5i所示。

標(biāo)識(shí)定位分為兩步，一是在Hamming解碼時(shí)通過(guò)Hamming校驗(yàn)計(jì)算獲得標(biāo)識(shí)相對(duì)正方位的旋轉(zhuǎn)角度[14]；二是通過(guò)已知的多個(gè)特征點(diǎn)解算圖像中標(biāo)識(shí)四邊形相對(duì)相機(jī)的位姿，其流程如圖6所示。

圖6 標(biāo)識(shí)位姿解算流程圖Fig.6 Identification pose calculation flow chart

為獲得標(biāo)識(shí)的主方向，定義距離計(jì)算函數(shù)為

(14)

式中mt——目標(biāo)標(biāo)識(shí)

mk——待識(shí)別標(biāo)識(shí)

n——標(biāo)識(shí)編碼格行、列數(shù)

?——異或操作符

旋轉(zhuǎn)因子rot將標(biāo)識(shí)逆時(shí)針旋轉(zhuǎn)4次，每次旋轉(zhuǎn)90°，當(dāng)H(mt，rot(mk))為零時(shí)即為標(biāo)識(shí)的主方向。

通過(guò)直接線(xiàn)性轉(zhuǎn)換(Direct linear transformation，DLT)算法求解標(biāo)識(shí)坐標(biāo)系相對(duì)相機(jī)坐標(biāo)系的變化矩陣。已知理想成像模型有

(15)

令hi=[h1ih2ih3i]T(i=1,2，3)，有

[h1h2h3]=sK[r1r2t]

(16)

對(duì)式(15)左右兩邊同乘像素坐標(biāo)向量的叉乘矩陣，有

式中O——零矩陣

展開(kāi)后可得到

表7詳細(xì)描述了較低聽(tīng)力水平班級(jí)在主測(cè)1中不同聽(tīng)力測(cè)試題型下的獨(dú)立樣本T檢驗(yàn)結(jié)果。和較高聽(tīng)力水平班級(jí)不同，較慢語(yǔ)速版本的測(cè)試材料可以使受試在短對(duì)話(huà)中得到明顯高的平均分?jǐn)?shù)(F=．159，Sig．=．691，t=2．763，p=．007＜0．05 ＜0．01)。相反，受試在長(zhǎng)對(duì)話(huà)(F=1．120，Sig．=．292，t=1．017，p=．311 ＞0．5)和短文(F=．193，Sig．=．661，t=．772，p=．442＞0．5)中的表現(xiàn)并沒(méi)有出現(xiàn)明顯差異。

(17)

其中

x=[h11h12h13h21h22h23h31h32h33]T

x存在8個(gè)未知量，構(gòu)建四組像素點(diǎn)信息，矩陣形式表示為

通過(guò)奇異值分解(Singular value decomposition，SVD)求解最小化‖Ax‖的優(yōu)化問(wèn)題，矩陣A最小奇異值對(duì)應(yīng)向量即為x解集。

3.2 組合標(biāo)識(shí)設(shè)計(jì)及位姿選取

單個(gè)標(biāo)識(shí)可能會(huì)因?yàn)椴蓸狱c(diǎn)被遮擋而導(dǎo)致識(shí)別失敗。故本文采取組合標(biāo)識(shí)的設(shè)計(jì)。其優(yōu)勢(shì)在于：①能夠增加對(duì)標(biāo)識(shí)遮擋的魯棒性。②能夠用于姿態(tài)計(jì)算的角點(diǎn)增多，提高定位精度。

組合標(biāo)識(shí)有二維、三維兩種方案，由于本文相機(jī)為俯視作業(yè)，采用二維組合標(biāo)識(shí)(圖7)。

圖7 二維組合標(biāo)識(shí)Fig.7 Two-dimensional combination identification

求解組合標(biāo)識(shí)的位姿，可先將子標(biāo)識(shí)中心點(diǎn)平移到組合標(biāo)識(shí)的中心

(18)

式中cMm,i——標(biāo)識(shí)相對(duì)相機(jī)的變換矩陣

oMm,i——每個(gè)標(biāo)識(shí)相對(duì)中心點(diǎn)的變換矩陣

(19)

式中Nm——檢測(cè)到的標(biāo)識(shí)數(shù)量

pi——檢測(cè)到的子標(biāo)識(shí)通過(guò)cMo,i的平移向量

根據(jù)文獻(xiàn)[16-18]將旋轉(zhuǎn)矩陣轉(zhuǎn)換為四元數(shù)q進(jìn)行求解

(20)

其中

4 卷?yè)P(yáng)系統(tǒng)隨動(dòng)控制策略

4.1 卷?yè)P(yáng)系統(tǒng)建模

卷?yè)P(yáng)機(jī)構(gòu)屬于單輸入單輸出系統(tǒng)(Single-input single-output，SISO)，基于SISO系統(tǒng)分別建立卷?yè)P(yáng)的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型。

卷?yè)P(yáng)機(jī)構(gòu)將執(zhí)行電機(jī)的旋轉(zhuǎn)運(yùn)動(dòng)轉(zhuǎn)換為鋼絲吊繩的直線(xiàn)運(yùn)動(dòng)。為簡(jiǎn)化模型，本文假設(shè)繩索具有足夠大的剛度，可將繩索看作移動(dòng)副剛體。伺服電機(jī)與吊繩之間的運(yùn)動(dòng)呈線(xiàn)性關(guān)系[19]

(21)

式中θw——卷筒旋轉(zhuǎn)角度

rw——卷筒半徑

z——吊繩下放位移

θm——電機(jī)輸出軸轉(zhuǎn)動(dòng)角

i——減速器傳動(dòng)比

由此可以得出，忽略繩索纏繞方式下的吊繩升降的期望線(xiàn)速度等效于卷筒的旋轉(zhuǎn)角或角速度，其關(guān)系為

(22)

式中Jz——系統(tǒng)雅可比系數(shù)

當(dāng)卷筒末端具有負(fù)載時(shí)，其輸出扭矩即為負(fù)載扭矩

Tw=TL=mgrw

(23)

在伺服電機(jī)輸出軸端的卷?yè)P(yáng)機(jī)械運(yùn)動(dòng)方程可表示為

(24)

(25)

式中Jeff——電機(jī)輸出軸端的系統(tǒng)等效轉(zhuǎn)動(dòng)慣量

Te——電機(jī)輸出轉(zhuǎn)矩

Beff——系統(tǒng)等效阻尼

Jm——電機(jī)轉(zhuǎn)子的轉(zhuǎn)動(dòng)慣量

Jd——卷筒和繩索的等效轉(zhuǎn)動(dòng)慣量

m——所吊重物及繩索等效質(zhì)量

4.2 速度控制與軌跡規(guī)劃

速度環(huán)部分采用偽微分-前饋控制算法，采用PID反饋控制設(shè)計(jì)前饋控制器，通過(guò)調(diào)節(jié)增益參數(shù)保證速度控制的無(wú)超調(diào)并提高對(duì)干擾的魯棒性。整體的控制算法流程框圖如圖8所示。

圖8 卷?yè)P(yáng)速度控制算法控制框圖Fig.8 Hoisting speed control algorithm block diagram

圖中KT為電機(jī)常數(shù)，rw為卷筒半徑，Kv,i為速度偽微分反饋算法的積分增益，Kv,p為比例增益。根據(jù)控制框圖，速度環(huán)的傳遞函數(shù)可表示為

(26)

通過(guò)調(diào)節(jié)速度環(huán)的前饋增益Kv,f、比例增益Kv,p以及積分增益Kv,i來(lái)調(diào)節(jié)其響應(yīng)特性。偽微分反饋各參數(shù)的計(jì)算可通過(guò)陳留法或Phelan法[20-21]設(shè)計(jì)為

(27)

式中r0——階躍信號(hào)最大幅值

mmax——控制輸出限幅

采用雙S形曲線(xiàn)對(duì)已知始末位置的運(yùn)動(dòng)進(jìn)行規(guī)劃，其次在該曲線(xiàn)的基礎(chǔ)上實(shí)現(xiàn)運(yùn)動(dòng)跟隨中的卷?yè)P(yáng)速度過(guò)渡以及加減速?zèng)Q策。加加速度邊界值變化7段式S形曲線(xiàn)如圖9所示。

圖9 雙S形曲線(xiàn)運(yùn)動(dòng)規(guī)劃Fig.9 Double S-curve motion planning

假設(shè)運(yùn)動(dòng)的初始狀態(tài)為(s0,v0)，時(shí)間T后的目標(biāo)狀態(tài)為(sf,vf)。根據(jù)該距離以及設(shè)定的速度，各規(guī)劃階段的加加速度、加速度、速度和位移之間的關(guān)系可遞推表示如下：

(1)加速段加加速

(28)

(2)加速段勻加速

(29)

(3)加速段加減速

(30)

(4)勻速段

(31)

(5)減速段減加速

(32)

(6)減速段勻減速

(33)

(7)減速段減減速

(34)

其中速度上限vlim=vf+jt45(t47-t45)。

對(duì)于運(yùn)動(dòng)的目標(biāo)可以對(duì)運(yùn)動(dòng)規(guī)劃過(guò)程進(jìn)行簡(jiǎn)化[22]。假設(shè)加速度變化未達(dá)到上限，即t12=t56=0，此時(shí)運(yùn)動(dòng)規(guī)劃可劃分為5段，這種方式的速度和加速度曲線(xiàn)仍連續(xù)。設(shè)采樣周期為T(mén)，加速階段和減速階段如圖10所示。

圖10 雙S形曲線(xiàn)加減速運(yùn)動(dòng)規(guī)劃Fig.10 Double S-curve acceleration and deceleration motion planning

由圖10可知，加速過(guò)程和減速過(guò)程分別為從加速到勻速和減速到勻速的過(guò)程，結(jié)合S形運(yùn)動(dòng)規(guī)劃曲線(xiàn)，加速階段為

(35)

減速階段為

(36)

式中tm——加速度的加速或減速時(shí)間

tc——加速度的巡航時(shí)間

5 試驗(yàn)及結(jié)果分析

試驗(yàn)平臺(tái)主要由上下2個(gè)Stewart六自由度平臺(tái)和包括卷?yè)P(yáng)機(jī)構(gòu)在內(nèi)的吊裝機(jī)械臂組成，如圖11所示。

圖11 海洋物流裝備綜合試驗(yàn)平臺(tái)Fig.11 Marine logistics equipment comprehensive experimental platform

5.1 靜態(tài)目標(biāo)下卷?yè)P(yáng)升降試驗(yàn)

為了驗(yàn)證具有完整運(yùn)動(dòng)規(guī)劃周期內(nèi)卷?yè)P(yáng)升降控制策略的可行性，本文首先設(shè)計(jì)了靜態(tài)目標(biāo)下的卷?yè)P(yáng)升降試驗(yàn)，首先位于卷?yè)P(yáng)上的彩色攝像機(jī)檢測(cè)作業(yè)環(huán)境中的靜止目標(biāo)標(biāo)識(shí)，在識(shí)別后對(duì)其進(jìn)行定位，獲取其相對(duì)位姿關(guān)系；其次根據(jù)手眼關(guān)系獲得在豎直方向上卷?yè)P(yáng)到目標(biāo)標(biāo)識(shí)的距離。

經(jīng)計(jì)算及多次調(diào)試，本文所提出的控制算法各增益參數(shù)最終分別為：位置環(huán)比例增益Kp為900，速度前饋增益Kv，f為60，加速度前饋增益Kv，f為4，速度環(huán)比例增益Kv,p為0.3，積分增益Kv,i為40，前饋增益Kv,f為0.05。根據(jù)試驗(yàn)?zāi)康模摳叨燃礊槔K索需下放的距離，由雙S形曲線(xiàn)運(yùn)動(dòng)規(guī)劃算法生成運(yùn)動(dòng)軌跡，根據(jù)該軌跡離散化后的電機(jī)規(guī)劃軌跡如圖12、13所示。

圖12 卷?yè)P(yáng)升降控制雙S形曲線(xiàn)(規(guī)劃)位移曲線(xiàn)Fig.12 Displacement curve of hoist lifting control with double S-curve

圖13 規(guī)劃與反饋速度曲線(xiàn)Fig.13 Error diagram of planning curve and feedback curve

根據(jù)試驗(yàn)結(jié)果可以看到，雙S形曲線(xiàn)規(guī)劃算法可以實(shí)現(xiàn)合理規(guī)劃及平穩(wěn)啟停。由圖12、13可以看出，在復(fù)合算法控制下，規(guī)劃位移曲線(xiàn)誤差較小，位置跟隨較為緊密，反饋速度變化曲線(xiàn)在運(yùn)動(dòng)控制過(guò)程中無(wú)較大速度突變，從速度規(guī)劃上有效避免了過(guò)程震蕩。綜上，本文提出的控制策略具有一定的可行性。

5.2 動(dòng)態(tài)目標(biāo)下卷?yè)P(yáng)隨動(dòng)試驗(yàn)

動(dòng)態(tài)目標(biāo)跟隨試驗(yàn)是由Stewart平臺(tái)做升沉運(yùn)動(dòng)，在視覺(jué)定位下，卷?yè)P(yáng)在升沉運(yùn)動(dòng)方向?qū)δ繕?biāo)對(duì)象的運(yùn)動(dòng)進(jìn)行跟隨控制試驗(yàn)?？紤]電動(dòng)缸運(yùn)動(dòng)行程，假設(shè)下平臺(tái)的升沉運(yùn)動(dòng)輸入曲線(xiàn)為

h=120sin(0.99t)+230

(37)

由于主要研究卷?yè)P(yáng)在升沉方向的隨動(dòng)情況，本文采用時(shí)間序列的位姿輸入模式，僅有z軸方向按照上述曲線(xiàn)運(yùn)動(dòng)，其他位姿無(wú)輸入量，平臺(tái)通過(guò)洗出算法得到各個(gè)電動(dòng)缸輸入量。運(yùn)動(dòng)模擬模塊控制平臺(tái)使其先到達(dá)下平臺(tái)電動(dòng)缸運(yùn)動(dòng)行程中位，然后開(kāi)始根據(jù)運(yùn)動(dòng)曲線(xiàn)輸入實(shí)現(xiàn)z軸方向升沉運(yùn)動(dòng)模擬，平臺(tái)整體運(yùn)行時(shí)間約為75 s，運(yùn)動(dòng)模擬平臺(tái)z軸方向運(yùn)動(dòng)反饋位移如圖14所示。

圖14 運(yùn)動(dòng)模擬平臺(tái)升沉運(yùn)動(dòng)反饋曲線(xiàn)Fig.14 Heaving motion feedback curve of motion simulation platform

跟隨試驗(yàn)的整體過(guò)程如圖15所示。在滿(mǎn)足吊裝工作空間的前提下，視覺(jué)檢測(cè)模塊不斷檢測(cè)并識(shí)別運(yùn)動(dòng)中的視覺(jué)標(biāo)識(shí)，并通過(guò)手眼關(guān)系換算得到卷?yè)P(yáng)執(zhí)行機(jī)構(gòu)相對(duì)目標(biāo)在z軸方向上的坐標(biāo)，該視覺(jué)運(yùn)動(dòng)軌跡如圖15所示。當(dāng)卷?yè)P(yáng)運(yùn)動(dòng)控制模塊接收到隨動(dòng)控制開(kāi)始指令后，規(guī)劃算法根據(jù)位移變化，從當(dāng)前位置開(kāi)始進(jìn)行軌跡規(guī)劃并加減速?zèng)Q策，并將視覺(jué)周期內(nèi)重規(guī)劃的運(yùn)動(dòng)軌跡點(diǎn)作為執(zhí)行機(jī)構(gòu)在控制周期內(nèi)的控制量輸入。

圖15 卷?yè)P(yáng)隨動(dòng)控制結(jié)果Fig.15 Result of hoisting follow-up control

當(dāng)跟隨結(jié)束指令發(fā)出后，卷?yè)P(yáng)根據(jù)當(dāng)前速度規(guī)

劃減速段進(jìn)行減速運(yùn)動(dòng)，直至停止。圖16為執(zhí)行機(jī)構(gòu)反饋位移與視覺(jué)位移的誤差變化曲線(xiàn)，由該曲線(xiàn)可以看出，在跟隨開(kāi)始時(shí)，由于加速段的存在，位置誤差變化增大，而后速度逐漸增大，誤差逐漸縮?。辉诜€(wěn)定跟隨后，在0.5個(gè)平臺(tái)運(yùn)動(dòng)周期內(nèi)的跟隨誤差均方根約為7.27%，在跟隨結(jié)束時(shí)，由于減速段已無(wú)目標(biāo)量存在，故誤差逐漸增大。該試驗(yàn)結(jié)果證實(shí)了提出的隨動(dòng)控制策略的有效性。

圖16 跟隨誤差曲線(xiàn)Fig.16 Error curve of following motion

6 結(jié)論

(1)搭建了海洋物流裝備綜合試驗(yàn)平臺(tái)，設(shè)計(jì)了卷?yè)P(yáng)控制模塊和視覺(jué)檢測(cè)模塊，由視覺(jué)系統(tǒng)檢測(cè)組合標(biāo)識(shí)，通過(guò)運(yùn)動(dòng)學(xué)解算使卷?yè)P(yáng)處于正常工作位置，之后進(jìn)行坐標(biāo)變換，得到卷?yè)P(yáng)執(zhí)行機(jī)構(gòu)相對(duì)于標(biāo)識(shí)目標(biāo)對(duì)象的相對(duì)位姿，據(jù)此獲取卷?yè)P(yáng)執(zhí)行機(jī)構(gòu)相對(duì)目標(biāo)對(duì)象的高度。

(2)分別進(jìn)行了靜態(tài)目標(biāo)和動(dòng)態(tài)目標(biāo)下的卷?yè)P(yáng)升降試驗(yàn)，驗(yàn)證了雙S形曲線(xiàn)規(guī)劃算法可以實(shí)現(xiàn)合理規(guī)劃及平穩(wěn)啟停，驗(yàn)證了卷?yè)P(yáng)升降控制的可行性；在卷?yè)P(yáng)對(duì)目標(biāo)對(duì)象的運(yùn)動(dòng)跟隨控制中，能夠在穩(wěn)定跟隨后使卷?yè)P(yáng)有效且平穩(wěn)地跟隨目標(biāo)對(duì)象運(yùn)動(dòng)，驗(yàn)證了隨動(dòng)控制策略的有效性。