一种两运动船舶相对六自由度运动的检测装置的制作方法

本发明属于船舶海上补给检测机构，特别涉及一种两运动船舶相对六自由度运动的检测装置。

背景技术：

在开放的海洋环境中，由于海风、海浪、洋流的作用以及船舶自身航行运动的影响，在海上补给等作业中，两作业装备会产生相对运动从而增加作业的不确定因素，尤其在对尺寸和重量较大集装箱和导弹鱼雷的补给中，要求定位精确，一次性完成，两船的相对运动会对海上安全作业构成很大威胁，因而必须使用具有波浪补偿功能的靠绑补给装备来完成。目前，国内外大多数涉及的波浪补偿系统都是带有蓄能器的被动式的系统和单自由度的主动式波浪补偿系统，由于被动式的波浪补偿系统补偿行程有限，有严重的滞后现象，补偿效率低，装备占用空间大等缺点，目前已经逐步被淘汰。单自由度的主动式波浪补偿系统由于只能补偿升沉方向的运动，主要应用于深海采矿等作业中。在海上靠绑补给作业中，两船除了会有升沉方向的相对运动之外，还有横摇、纵摇和首摇三个姿态和两个水平方向的横荡、纵荡相对运动，如图1所示。在对集装箱等尺寸较大的货物补给过程中，货物与甲板间由于存在六个自由度的相对运动，单自由度的补给动作并不能满足要求，因此需要有六自由度波浪补偿功能的作业装备实现集装箱和导弹鱼雷等货物的补给目标。

在具有六自由度波浪补偿功能的作业装备中，两船(补给船和被补给船)都在做六自由度运动，首先要对两运动船舶的相对六自由度运动检测，这是精确控制的基础。在波浪补偿系统中，需要根据两船的相对运动来控制负载的位姿，要求对两个各自做六自由度船舶的相对位姿进行实时检测反馈到控制系统中，以达到对负载位姿的精确控制。对两运动船舶相对位姿检测包括横摇，纵摇、艏摇三个欧拉角和横荡、纵荡、升沉三个平移位量，两运动船舶相对位姿检测参数的精确性和实时性直接关系到整个波浪补偿系统控制的精度，因此，两运动船舶相对六自由度位姿检测是一项重要的工作。

技术实现要素：

本发明的目的是在波浪补偿作业中，提供一种检测精度高的两运动船舶相对六自由度运动的检测装置。

本发明提供的两运动船舶相对六自由度运动的检测装置，为基于双目视觉系统的两运动船舶相对六自由度运动的检测装置，包括设在补给船上的吊具系统、设在所述吊具系统上的不同位置的两台摄像机、图像采集和处理装置、设在被补给船上的棋盘格标记，所述摄像机通过复合支架设在所述吊具系统的基座上能够做左右上下摆动；首先对两摄像机进行空间位置标定，然后两个摄像机从不同角度，不同位置对所述棋盘格标记进行图像拍摄，图像数据经过图像采集和处理装置立体校正、立体匹配、深度提取与三维位姿估计来计算出所述吊具系统与被补给船之间的相对六自由度运动。

本发明提供的两运动船舶相对六自由度运动的检测装置，为基于Mti(Motion Tracker Instrument)与激光传感器的两运动船舶相对六自由度运动的检测装置，包括设在补给船上的吊具系统、设在所述吊具系统上的三台不在同一直线上的激光传感器和一台Mti、图像采集和处理装置、设在被补给船上的稳定器和另一台Mti，所述激光传感器通过复合支架设在所述吊具系统的基座上能够做左右上下摆动；所述三个激光传感器分别测量固定在被补给船上稳定器的三个面与激光传感器的距离，再按照几何的方法解算出所述吊具系统与被补给船的三个相对平移量；两个MTi分别检测所述吊具系统与被补给船的姿态，再用静力学方法解算出吊具系统与被补给船的三个相对旋转运动。

所述复合支架包括设在所述基座上的第一支架、设在所述第一支架上能够做水平旋转的第二支架，所述摄像机或激光传感器以上下摆动方式设在所述第二支架上。

有益效果

本发明具有如下特点：一是基于双目视觉系统可以实现两个运动船舶横荡、纵荡、升沉、横摇、纵摇、艏摇相对六自由度运动量检测；二是摄像机可以左右上下摆动，扩大视野范围；三是无需在被补给船上安装设备，只需放上一块标记板即可，达到无接触测量目的；四是基于激光传感器与Mti相结合实现两个运动船舶横荡、纵荡、升沉、横摇、纵摇、艏摇相对六自由度运动量检测；五是在被补给船旋转的情况下，激光传感器也能精确的检测出两运动船舶的相对位移；六是激光传感器可以左右上下摆动。总之，本发明具有检测精度高，适用性好的优点。

下面结合附图详细说明本发明。

附图说明

图1是两运动船舶波浪补偿作业示意图。

图2是本发明基于双目视觉系统的两运动船舶相对六自由度运动的检测装置的结构示意图。

图3是基于Mti(Motion Tracker Instrument)与激光传感器的两运动船舶相对六自由度运动的检测装置的结构示意图。

图4是本发明装置中吊具系统的钢丝绳长度求解向量示意图。

图5是本发明中双目视觉系统硬件设计示意图。

图6是本发明中激光传感器检测相对位移原理示意图。

图7是本发明中被测量面旋转示意图。

图8是本发明中MTi检测两运动物体相对姿态示意图。

图9是本发明中MTi与激光传感器相结合检测装置硬件设计示意图。

图10是本发明中MTi与激光传感器相结合检测装置软件设计示意图。

具体实施方式

两运动船舶(补给船和被补给船)波浪补偿作业如图1所示，作业时补给船通过吊具系统S将负载T向被补给船进行补给。

本发明的实施方式一为基于双目视觉系统的两运动船舶相对六自由度运动的检测装置。双目视觉是机器视觉的一个重要分支，它是基于视差原理，由不同位置的两台摄像机或者一台摄像机移动或旋转对同一幅场景进行拍摄，通过计算空间点在两幅图像间的位置偏差，来获取物体三维几何信息的方法。图2示出了本发明检测装置的总体结构。检测装置主要由PC机、两个CCD工业摄像机5、6、棋盘格标记7、图像数据采集卡、路由器、支架2、支架3等组成。两个摄像机5、6分别通过支架3安装在支架2上，支架2安装在吊具系统S的基座1上。支架3可以相对支架2绕垂直轴做水平旋转，摄像机5、6可以沿支架3上的导槽4做上下摆动。支架2和支架3的组合可以实现摄像机5、6的左右上下摆动，以适应不同物体大小而调节视角大小。棋盘格标记7安装在被补给船上，位置偏离负载T落点位置。

基于双目视觉系统的两运动船舶相对六自由度运动的检测装置工作方式是：首先对两摄像机5、6进行空间位置标定，然后两个摄像机5、6从不同角度，不同位置对棋盘格标记7进行图像拍摄，图像数据经过图像数据采集卡输入到PC机中，PC机经过立体校正、立体匹配、深度提取与三维位姿估计来计算出吊具系统S与被补给船之间的相对六自由度运动。

本发明的实施方式二是基于Mti(Motion Tracker Instrument)与激光传感器的两运动船舶相对六自由度运动的检测装置。MTi是荷兰Xsens公司生产的一款微型的可以精准测量姿态和航向以及惯性参数的产品。MTi利用其内部的增强型三轴陀螺仪快速跟踪被测物体的空间姿态，同时测量3个轴向的加速度和地磁场为3轴转速提供补偿，其内部集成的传感器融合算法，通过USB串行通信总线接口实时地向系统准确、可靠地提供载体的航向和姿态信息，具有体积小、重量轻、功耗低、成本低等优点。

图3为本发明实施方式二的总体结构：主要由PC机、RS485转换器、RS232转换器、三个激光传感器8、9、10、稳定器12、两个MTi(MTi1、MTi2)、支架2、支架3和支架11等组成。

三个激光传感器通过支架3安装在支架2上，支架2安装在吊具系统S的基座1上。支架3可以相对支架2绕垂直轴做水平旋转，激光传感器8、9、10可以沿支架3上的槽导4做上下摆动。支架2和支架3的组合可以实现激光传感器8、9、10左右上下摆动，以激光点照射位置。稳定器12安装在支架11上，支架11通过磁铁吸附在被补给船的甲板上。MTi1安装在吊具系统S的基座1上，MTi2安装在被补给船的甲板上。稳定器12的作用是保持稳定器三个被测量的平面不受被补给船横摇，纵摇、艏摇三个运动旋转，稳定器始终保持三个被测量平面保持在原有位置，消除了因测量平面与激光传感器之间的夹角变化而引起的误差，这个误差会造成激光传感器测量距离的变化。

激光传感器与MTi相结合检测相对六自由度的工作方式为：三个激光传感器分别检测吊具系统与被补给船之间的三个相对平移量，两个MTi(MTi1、MTi2)检测三个相对姿态，即三个角度。三个激光传感器8、9、10分别测量固定在被补给船上稳定器的三个面与激光传感器的距离，再按照几何的方法解算出吊具系统S与被补给船的三个相对平移量。MTi1和MTi2分别检测吊具系统S与被补给船的姿态，再用静力学方法解算出吊具系统与被补给船的三个相对旋转运动。

本发明进一步的说明如下：

1)波浪补偿位移补偿原理

位移补偿的目标是在并靠补给过程中保证负载相对于被补给船的位置和姿态不受两船相对运动的影响。通过安装在吊具系统上的测量系统，可以得到不同时刻被补给船相对于基座的位置和姿态假设负载相对于基座的位置和姿态和相对于被补给船的期望位置和姿态设定为和那么为了使负载相对于被补给船的位姿不受两船相对运动的影响，需要利用绳牵引并联机构实时控制负载相对于基座的位姿，使其在任意时刻都满足

在这种情况下负载相对被补给船的位姿就是预设的目标值和与两船的相对运动无关。对于绳牵引并联机构，负载相对于基座的位姿是通过改变绳索长度来控制的。根据式(1)和(2)可以计算出任一时刻负载相对基座的位姿，结合绳牵引并联机构的运动学理论，就可以得到该时刻各绳索的目标长度。

如图4所示，P_i和B_i(i＝1,…,m)分别表示第i根绳索与负载和基座的连接点，p_i和b_i表示从负载和基座坐标系的原点指向相应的绳索连接点的矢量，也就是l_i为第i根绳索对应的矢量，e_i为l_i的单位矢量。根据(h)中的几何关系可知，在坐标系B中

由于和是恒定的，根据式(1)和(2)得到和后，就可以利用式(3)计算从而得到第i根绳索的长度

因此在并靠补给过程中，只要给定负载相对于被补给船的期望位姿，并测量出被补给船相对基座的位姿，就可以根据式(4)计算出位移补偿需要的绳索长度。然后实时控制各绳索的收放使其长度与计算值相等，就能够保证负载与被补给船的相对位姿与预先设定的期望值一致，与两船的相对运动无关。

2)吊具系统速度补偿原理

对式(1)两边求导可得

根据刚体运动学理论，方向余弦矩阵的求导公式为

其中为的斜对称矩阵。假设ω＝[ω_x ω_y ω_z]^T，那么

斜对称矩阵具有如下性质：

[ω×]υ＝ω×υ (8)

其中ω和υ表示任意两个矢量。

一个坐标系相对于另一个坐标系的角速度可以根据两坐标系间的欧拉角及其导数计算出来，也就是

其中

将式(6)和(9)代入式(5)，可以得到负载与基座相对速度的计算公式

可以看出，负载相对于被补给船的速度与负载相对于基座的速度、负载相对于被补给船的位置以及被补给船相对于基座的速度、欧拉角及其导数有关。

绳牵引并联机构负载相对基座的角速度计算公式为

可以看出，负载相对于被补给船的角速度与负载相对于基座的角速度以及被补给船相对于基座的姿态和角速度有关。因此，为了保证负载与被补给船的相对速度为预先设定的值而不受两船相对运动的影响，负载与基座之间的运动旋量必须要满足式(11)和(12)。

对式(4)两边平方然后求导可得

将式(3)代入式(13)并化简得

与式(6)类似，方向余弦矩阵的求导公式还有另外一种形式

将式(15)代入式(14)可以得到

根据向量混合积的性质有

那么式(16)可以重新写为

定义l＝[l₁ l₂ … l_m]^T为绳索长度矢量，将所有绳索对应的式(18)综合到一起可得

式中

称为绳牵引并联机构的雅可比矩阵，是在坐标系B中表示的，只与绳牵引并联机构的几何参数和负载相对于基座的位姿有关，与基座在惯性坐标系中的位姿无关。

控制绳牵引并联机构各绳索的收放速度，使其在任意时刻都满足式(19)，那么负载相对于被补给船的运动速度就等于预先设定值而与两船相对运动无关，这就是速度补偿原理。

3)立体校正与立体匹配

立体校正的目的是使两台摄像机的光轴平行，从而保证成像平面中的左右两幅图像是行对准的。一旦有了旋转或者平移值(R,T),就可以利用这些结果来校正两幅立体图像，使得极线沿着图像行对准，并且穿过两幅图像的扫描线也是相同的。实际情况是左右两台摄像机几乎不可能出现准确的行对准的成像平面。只在图像的一行上面搜索另一图像的匹配点能够提高匹配的可靠性和算法效率。让每个图像平面都落在一个公共成像面上并水平对准的结果是极点都位于无穷远。

本发明采用Bouguet算法(即标定立体校正)，它使用两台摄像机的旋转和平移参数。Bouguet算法在给定旋转矩阵和平移向量(R,T)之后，使每一幅图像的重投影次数最小化和观测面积最大化。已知屏幕坐标和摄像机内参数矩阵，二维点投影到三维中的重投影矩阵如下：

上式中，c_x'为主点在右图像中的x坐标，其他参数都来自左图像。c_x＝c_x'的条件是主光线相交于无穷远处。将一个已知的二维齐次点(其关联视差d)投影到三维中：

三维坐标就是(X/W,Y/W,Z/W)。

双目立体视觉打算通过两幅图像来获取物体的三维坐标信息，立体匹配是其关键技术，首先找到两幅图像中的对应点，然后计算点的视差和深度信息。

本发明是检测目标平面的位姿变化，故采用关键点匹配法。选择棋盘格标记板作为关键点，由于OpenCV中针对棋盘格内角点检测的函数findChessboardCorners()对角点进行了排序，所以对于校正过的图像，可以直接选择编号相同的点作为匹配点，节省了时间，提高了效率。

4)深度提取与三维位姿估计

完成匹配之后，得到了关键点的视差(d＝x_l-x_r),将图像特征点P(x_l，y_l)以及视差d带入式(22)中，就得到空间点P在左摄相机坐标系中的空间坐标(X,Y,Z)。Z值即为深度信息。

三维位姿估计，计算的是目标平面在左摄相机坐标系下的空间三维位姿，本实验采用的是OpenCV中的Solvepnp算法，立体匹配之后得出标记点的图像坐标和空间坐标，选取至少四点(非共线)，就可求解出空间平面的位姿。

Solvepnp调用的是cvFindExtrinsicCameraParams2通过已知的内参进行未知外参求解，是一个精确解。其基本原理如下：

Z_c＝X_w*R₃₁+Y_w*R₃₂+Z_w*R₃₃+T (26)

将(26)式代入(24)式和(25)式，整理后，至少需要4个点，8个方程就能得到R,T矩阵，求出目标平面的位姿。

4)双目视觉检测系统的硬件设计

如图5所示，两个摄像机采集棋盘格标记图像信号，通过网线把数据输入到路由器中，路由器把数据汇总输入到千兆网卡GigE-2中，网卡与PC机主板通过PCI接口连接。

5)激光传感器检测三个相对平移量原理

如图6所示，为激光传感器检测三个平移量原理。激光传感器8检测升沉，激光点射在稳定器的上平面上，激光传感器10检测纵荡，激光点射在稳定器的右平面上，激光传感器9检测横荡，激光点射在稳定器的前平面上。以激光传感器10检测纵荡为例，目的是检测吊具系统的中心O与被补给船中心P之间的纵向相对位移，即e的大小。安装距离a，安装角度β，激光传感器测试距离b，被测量平面到中心P距离d都为已知量，则利用几何关系可得

e＝d-(a-b·cosβ) (27)

其它两个方向同理。

6)稳定器作用原理

当被补给船相对补给船只作旋转运动时，视为两者的相对平移量为0，也就是说，旋转前与旋转后激光传感器检测到的纵荡都应为式(27)。如图7所示，为被测量平面跟随被补给船不旋转时，激光传感器测量到的距离为b1，当被测量平面跟随被补给船旋转时，激光传感器测量到的距离为b2，显然b＝b1≠b2，可见，被测量平面跟随被补给船旋转会影响激光传感器检测到的距离，从而影响精度。稳定器的作是保持被测量平面与激光传感器激光入射角θ不变，保证被补给船的旋转不会影响d值的测量。

7)MTi检测三个相对旋转量原理

如图8所示，为两个运动物体相对姿态模型。由已知的模块A和模块B的姿态角ψ_A,θ_A,φ_A,ψ_B,θ_B,φ_B，就可以计算出它们之间的相对姿态角ψ_AB,θ_AB,φ_AB，即

相对姿态为

8)激光传感器与MTi结合检测相对六自由度软硬件设计如图9所示，为激光传感器与MTi结合检测相对六自由度硬件设计。MTi分别检测补给船和被补给船的姿态，信号经过RS232转换器，转换为USB接口信号，输入PC机中。激光传感器分别检测补给船和被补给船的相对升沉、纵荡和横荡，信号经过RS485转换器，转换为USB接口信号，输入PC机中。图10为激光传感器与MTi结合检测相对六自由度软件设计。