基于视觉的吊装机器人卷扬随动控制研究

徐春博 赵丁选,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形曲线规划算法可以实现合理规划及平稳启停，验证了卷扬升降控制的可行性；在卷扬对目标对象的运动跟随控制中，能够在稳定跟随后使卷扬有效且平稳地跟随目标对象运动，验证了随动控制策略的有效性。