基于視覺的吊裝機器人卷揚隨動控制研究

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

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

0 引言

在海洋浪涌的影響下，船舶產生了橫傾、縱傾和升沉等運動，海洋物流吊裝設備的安全性、可靠性和工作效率受到了嚴重的影響。目前主要通過海浪補償、吊重減擺和主動軌跡規劃等方法實現吊運的智能控制。

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

上述研究大多采用MRU對補償量進行測量，其價格昂貴，具有一定的時滯性，并且此種方式在作業過程中需要人工輸入貨物位置信息，應用成本較高。本文將計算機視覺引入海浪補償問題中，提出一種基于視覺的卷揚隨動控制系統，并嘗試將運動補償中保持相對靜止的問題轉換為運動跟隨問題進行研究。該系統依托于實驗室搭建的海洋物流裝備智能控制模擬試驗平臺，采用機器視覺的方式，在手眼關系的基礎上，實現對目標位置的定位與跟蹤，進而控制吊裝機械臂卷揚升降，實現隨動控制。

1 系統組成

圖1為卷揚隨動控制系統，主要依托于搭建的綜合試驗平臺，其主要包括運動模擬平臺、吊裝機械臂、視覺相機及標識、卷揚機構等。運動模擬平臺可根據海浪譜模型生成的波浪曲線模擬海浪運動對船舶造成的橫搖、縱搖、垂蕩等影響；卷揚機構安置于吊裝機械臂的小臂末端，機械臂和卷揚整體模擬起重設備，實現折臂式起重機的回轉、起吊、前伸等運動的自動控制，使末端卷揚到達指定的吊裝點上方。系統的控制流程框圖如圖2所示。

圖2 基于視覺的吊裝機械臂卷揚隨動控制框圖Fig.2 Block diagram of vision-based hoisting robot follow-up control system

如圖2所示，控制系統主要分為視覺模塊和運動控制模塊。視覺模塊在組合標識知識庫標識、相機模型、卷揚執行機構和卷筒之間的手眼關系等前提信息基礎下，實現對組合標識的檢測和定位，并且通過機械臂運動學使卷揚處于正常的工作位置。在視覺系統檢測到標識后，首先采用四元數法以及平均權值向量法實現子標識的位姿融合，其次進行坐標變換，得到了卷揚執行機構相對于標識目標對象的相對位姿，最后據此獲取卷揚執行機構相對目標對象的高度。運動控制模塊通過卷揚的運動雅可比函數將運動控制量從笛卡爾空間變換到關節空間中，采用速度環反饋控制的方式，實現對關節控制量的連續控制。

2 運動學分析及手眼標定

2.1 機器人正運動學

采用Kuka R16六自由度機械臂模擬折臂式起重機結構。該機器人前三關節中，大臂模擬折臂式起重機的主臂，小臂模擬折臂式起重機的輔助臂，采用文獻[10]的約定對吊裝機械臂的運動學進行建模，如圖3所示。其中，吊裝機械臂的末端為卷揚系統在機械臂小臂上的固定點，其坐標系為{O4}，卷揚系統即末端執行器的坐標系為{O5}，這兩個坐標系均為相對前一個坐標系固定的位姿，坐標系{O5}不隨卷筒的轉動而變化。

圖3 吊裝機器人及卷揚幾何坐標Fig.3 Hoisting robot and hoist geometric cooridnates

為方便從關節角3到末端執行器坐標系的建立

與坐標變換的推導，將末端和末端執行器也按照上述關節參數的定義統一到D-H表，其中關節3到末端坐標系{O4}之間的距離作為桿件4，末端{O4}到末端執行器{O5}的距離作為桿件5，根據圖3建立的D-H參數如表1所示。

表1 機器人D-H參數Tab.1 D-H parameters of robots

2.2 機器人逆運動學

根據文獻[11]的矩陣逆乘解析法求逆解。由正運動學已知末端執行器相對基座總變換矩陣為

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。

通過整理得到

令

m=nxny-oxoy-axay

解得

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

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

2.3 手眼系統標定

對機械臂進行兩次旋轉軸非平行的位姿變換可得到相機相對參考物的坐標變換oHc,1、oHc,2，機械臂末端執行器相對基座的坐標變換bHt,1、bHt,2，可構建具有封閉環的坐標轉換

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

(5)

其中

進行多次測量以消除測量噪聲帶來的誤差，并進行整體迭代優化求解

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]對X分步求解

(7)

(8)

其在李群中的對數映射為

(9)

其中

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

(10)

由式(10)轉換為最小二乘擬合問題，即

(11)

得到RX的最優解為

(12)

其中

最后求得X的平移向量為

tX=(CTC)-1CTD

(13)

其中

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

圖4 手眼標定采集圖像Fig.4 Hand-eye calibration image acquisition

最終得到攝像頭與卷揚卷筒之間的手眼關系坐標變換矩陣為

3 基于視覺標識的目標定位跟蹤

3.1 標識的識別與定位

標識識別主要包括圖像預處理與解碼。本小節以AprilTags標識為例，對其進行檢測識別算法處理，處理流程如圖5所示。

圖5 復雜環境下標識檢測識別流程圖Fig.5 Identification detection flow chart in complex environment

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

基于Hamming距離對Apriltags標識進行解碼識別，將預定義的Hamming編碼以哈希表形式生成并對編碼信息序列進行校驗，最終獲得具有唯一編碼的識別號，檢測結果如圖5h所示。

為了提高定位精度，對原圖四邊形區域邊緣梯度進行提取，沿著垂直邊緣法線方向朝標識內查找梯度值最大的位置排除噪點。以邊緣梯度為權重，對法線方向的點進行加權平均，對同一邊的加權平均點做線段擬合，獲得標識細化的邊緣，識別效果如圖5i所示。

標識定位分為兩步，一是在Hamming解碼時通過Hamming校驗計算獲得標識相對正方位的旋轉角度[14]；二是通過已知的多個特征點解算圖像中標識四邊形相對相機的位姿，其流程如圖6所示。

圖6 標識位姿解算流程圖Fig.6 Identification pose calculation flow chart

為獲得標識的主方向，定義距離計算函數為

(14)

式中mt——目標標識

mk——待識別標識

n——標識編碼格行、列數

?——異或操作符

旋轉因子rot將標識逆時針旋轉4次，每次旋轉90°，當H(mt，rot(mk))為零時即為標識的主方向。

通過直接線性轉換(Direct linear transformation，DLT)算法求解標識坐標系相對相機坐標系的變化矩陣。已知理想成像模型有

(15)

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

[h1h2h3]=sK[r1r2t]

(16)

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

式中O——零矩陣

展開后可得到

表7詳細描述了較低聽力水平班級在主測1中不同聽力測試題型下的獨立樣本T檢驗結果。和較高聽力水平班級不同，較慢語速版本的測試材料可以使受試在短對話中得到明顯高的平均分數(F=．159，Sig．=．691，t=2．763，p=．007＜0．05 ＜0．01)。相反，受試在長對話(F=1．120，Sig．=．292，t=1．017，p=．311 ＞0．5)和短文(F=．193，Sig．=．661，t=．772，p=．442＞0．5)中的表現并沒有出現明顯差異。

(17)

其中

x=[h11h12h13h21h22h23h31h32h33]T

x存在8個未知量，構建四組像素點信息，矩陣形式表示為

通過奇異值分解(Singular value decomposition，SVD)求解最小化‖Ax‖的優化問題，矩陣A最小奇異值對應向量即為x解集。

3.2 組合標識設計及位姿選取

單個標識可能會因為采樣點被遮擋而導致識別失敗。故本文采取組合標識的設計。其優勢在于：①能夠增加對標識遮擋的魯棒性。②能夠用于姿態計算的角點增多，提高定位精度。

組合標識有二維、三維兩種方案，由于本文相機為俯視作業，采用二維組合標識(圖7)。

圖7 二維組合標識Fig.7 Two-dimensional combination identification

求解組合標識的位姿，可先將子標識中心點平移到組合標識的中心

(18)

式中cMm,i——標識相對相機的變換矩陣

oMm,i——每個標識相對中心點的變換矩陣

(19)

式中Nm——檢測到的標識數量

pi——檢測到的子標識通過cMo,i的平移向量

根據文獻[16-18]將旋轉矩陣轉換為四元數q進行求解

(20)

其中

4 卷揚系統隨動控制策略

4.1 卷揚系統建模

卷揚機構屬于單輸入單輸出系統(Single-input single-output，SISO)，基于SISO系統分別建立卷揚的運動學和動力學模型。

卷揚機構將執行電機的旋轉運動轉換為鋼絲吊繩的直線運動。為簡化模型，本文假設繩索具有足夠大的剛度，可將繩索看作移動副剛體。伺服電機與吊繩之間的運動呈線性關系[19]

(21)

式中θw——卷筒旋轉角度

rw——卷筒半徑

z——吊繩下放位移

θm——電機輸出軸轉動角

i——減速器傳動比

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

(22)

式中Jz——系統雅可比系數

當卷筒末端具有負載時，其輸出扭矩即為負載扭矩

Tw=TL=mgrw

(23)

在伺服電機輸出軸端的卷揚機械運動方程可表示為

(24)

(25)

式中Jeff——電機輸出軸端的系統等效轉動慣量

Te——電機輸出轉矩

Beff——系統等效阻尼

Jm——電機轉子的轉動慣量

Jd——卷筒和繩索的等效轉動慣量

m——所吊重物及繩索等效質量

4.2 速度控制與軌跡規劃

速度環部分采用偽微分-前饋控制算法，采用PID反饋控制設計前饋控制器，通過調節增益參數保證速度控制的無超調并提高對干擾的魯棒性。整體的控制算法流程框圖如圖8所示。

圖8 卷揚速度控制算法控制框圖Fig.8 Hoisting speed control algorithm block diagram

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

(26)

通過調節速度環的前饋增益Kv,f、比例增益Kv,p以及積分增益Kv,i來調節其響應特性。偽微分反饋各參數的計算可通過陳留法或Phelan法[20-21]設計為

(27)

式中r0——階躍信號最大幅值

mmax——控制輸出限幅

采用雙S形曲線對已知始末位置的運動進行規劃，其次在該曲線的基礎上實現運動跟隨中的卷揚速度過渡以及加減速決策。加加速度邊界值變化7段式S形曲線如圖9所示。

圖9 雙S形曲線運動規劃Fig.9 Double S-curve motion planning

假設運動的初始狀態為(s0,v0)，時間T后的目標狀態為(sf,vf)。根據該距離以及設定的速度，各規劃階段的加加速度、加速度、速度和位移之間的關系可遞推表示如下：

(1)加速段加加速

(28)

(2)加速段勻加速

(29)

(3)加速段加減速

(30)

(4)勻速段

(31)

(5)減速段減加速

(32)

(6)減速段勻減速

(33)

(7)減速段減減速

(34)

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

對于運動的目標可以對運動規劃過程進行簡化[22]。假設加速度變化未達到上限，即t12=t56=0，此時運動規劃可劃分為5段，這種方式的速度和加速度曲線仍連續。設采樣周期為T，加速階段和減速階段如圖10所示。

圖10 雙S形曲線加減速運動規劃Fig.10 Double S-curve acceleration and deceleration motion planning

由圖10可知，加速過程和減速過程分別為從加速到勻速和減速到勻速的過程，結合S形運動規劃曲線，加速階段為

(35)

減速階段為

(36)

式中tm——加速度的加速或減速時間

tc——加速度的巡航時間

5 試驗及結果分析

試驗平臺主要由上下2個Stewart六自由度平臺和包括卷揚機構在內的吊裝機械臂組成，如圖11所示。

圖11 海洋物流裝備綜合試驗平臺Fig.11 Marine logistics equipment comprehensive experimental platform

5.1 靜態目標下卷揚升降試驗

為了驗證具有完整運動規劃周期內卷揚升降控制策略的可行性，本文首先設計了靜態目標下的卷揚升降試驗，首先位于卷揚上的彩色攝像機檢測作業環境中的靜止目標標識，在識別后對其進行定位，獲取其相對位姿關系；其次根據手眼關系獲得在豎直方向上卷揚到目標標識的距離。

經計算及多次調試，本文所提出的控制算法各增益參數最終分別為：位置環比例增益Kp為900，速度前饋增益Kv，f為60，加速度前饋增益Kv，f為4，速度環比例增益Kv,p為0.3，積分增益Kv,i為40，前饋增益Kv,f為0.05。根據試驗目的，該高度即為繩索需下放的距離，由雙S形曲線運動規劃算法生成運動軌跡，根據該軌跡離散化后的電機規劃軌跡如圖12、13所示。

圖12 卷揚升降控制雙S形曲線(規劃)位移曲線Fig.12 Displacement curve of hoist lifting control with double S-curve

圖13 規劃與反饋速度曲線Fig.13 Error diagram of planning curve and feedback curve

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

5.2 動態目標下卷揚隨動試驗

動態目標跟隨試驗是由Stewart平臺做升沉運動，在視覺定位下，卷揚在升沉運動方向對目標對象的運動進行跟隨控制試驗。考慮電動缸運動行程，假設下平臺的升沉運動輸入曲線為

h=120sin(0.99t)+230

(37)

由于主要研究卷揚在升沉方向的隨動情況，本文采用時間序列的位姿輸入模式，僅有z軸方向按照上述曲線運動，其他位姿無輸入量，平臺通過洗出算法得到各個電動缸輸入量。運動模擬模塊控制平臺使其先到達下平臺電動缸運動行程中位，然后開始根據運動曲線輸入實現z軸方向升沉運動模擬，平臺整體運行時間約為75 s，運動模擬平臺z軸方向運動反饋位移如圖14所示。

圖14 運動模擬平臺升沉運動反饋曲線Fig.14 Heaving motion feedback curve of motion simulation platform

跟隨試驗的整體過程如圖15所示。在滿足吊裝工作空間的前提下，視覺檢測模塊不斷檢測并識別運動中的視覺標識，并通過手眼關系換算得到卷揚執行機構相對目標在z軸方向上的坐標，該視覺運動軌跡如圖15所示。當卷揚運動控制模塊接收到隨動控制開始指令后，規劃算法根據位移變化，從當前位置開始進行軌跡規劃并加減速決策，并將視覺周期內重規劃的運動軌跡點作為執行機構在控制周期內的控制量輸入。

圖15 卷揚隨動控制結果Fig.15 Result of hoisting follow-up control

當跟隨結束指令發出后，卷揚根據當前速度規

劃減速段進行減速運動，直至停止。圖16為執行機構反饋位移與視覺位移的誤差變化曲線，由該曲線可以看出，在跟隨開始時，由于加速段的存在，位置誤差變化增大，而后速度逐漸增大，誤差逐漸縮小；在穩定跟隨后，在0.5個平臺運動周期內的跟隨誤差均方根約為7.27%，在跟隨結束時，由于減速段已無目標量存在，故誤差逐漸增大。該試驗結果證實了提出的隨動控制策略的有效性。

圖16 跟隨誤差曲線Fig.16 Error curve of following motion

6 結論

(1)搭建了海洋物流裝備綜合試驗平臺，設計了卷揚控制模塊和視覺檢測模塊，由視覺系統檢測組合標識，通過運動學解算使卷揚處于正常工作位置，之后進行坐標變換，得到卷揚執行機構相對于標識目標對象的相對位姿，據此獲取卷揚執行機構相對目標對象的高度。

(2)分別進行了靜態目標和動態目標下的卷揚升降試驗，驗證了雙S形曲線規劃算法可以實現合理規劃及平穩啟停，驗證了卷揚升降控制的可行性；在卷揚對目標對象的運動跟隨控制中，能夠在穩定跟隨后使卷揚有效且平穩地跟隨目標對象運動，驗證了隨動控制策略的有效性。