[发明专利]一种水下单目亚像素相对位姿估计方法

说明书

本发明提出一种水下单目亚像素相对位姿估计方法，首先，在目标对接装置上设置光源标靶，光源标靶上具有至少5个特征光源，事先获取特征光源中在世界坐标系下的三维坐标。其次，利用水下单目视觉系统采集光源标靶图像，获取像素坐标系下，图像中每个特征点的坐标。再次，以特征点在世界坐标系下的坐标和像素坐标系下的坐标作为输入，利用水下单目视觉系统相机的内参数矩阵，采用共面PnP算法，得到世界坐标系相对于相机坐标系的相对位姿矩阵^cM_w的粗值。最后，以得到的^cM_w粗值为初始值，采用RLS优化算法，迭代得到^cM_w精确值。本发明可以准确的得到相对位姿信息，完成水下机器人自主对接工作。

技术领域

本发明涉及水下机器人视觉技术领域，具体为一种水下单目亚像素相对位姿估计方法。

背景技术

机器人远距离长时间自主作业时，因为机器人本身负载和水下通讯限制，需要进行对接作业进入回收装置，进行数据传输和能源补给。长期以来，国内外学者在水下水声信号对接方面取得了很好的研究成果，但由于水声信号只在长距离位置上性能较好，近距离稳定性差、精度低、鲁棒性差，不适用于近距离对接作业中。为了满足自主对接作业的需要，人们需要实现近距离的位姿估计数据的获取，从而控制水下机器人进入对接装置。视觉传感器，以其低成本、高分辨率、高帧率、低噪声、低延迟等优点，在自主对接的相对位姿估计中具有独特的优势，适用于近距离、高精度的对接作业相对位姿估计。

传统常用的近距离视觉对接作业方法主要采用的是最简单的几何图形学方法来进行导引，一般以光源标靶的中心作为目标导引点，导引水下机器人按照中心点的方向进入对接装置，或者以目标光源点构成的图像面积、周长作为目标，通过其变化之间的关系来导引水下机器人进入对接装置。这类传统的近距离视觉对接作业方法存在一定的局限性和缺陷，主要表现为：首先，简单几何图形学的方法可用的特征较少，水下机器人对接过程中没有考虑到机器人本身姿态的影响，仅包含位置信息，在工程中会导致自主对接作业因为姿态错误最终失败；其次，简单几何图形学的方法在水下采集图像进行提取处理时，因为水下环境的复杂性，特征的提取会产生较大误差或者出现丢失的情况，鲁棒性极差，在工程中，会出现水下机器人丢失目标，数据误差太大，导致自主对接作业失败。

发明内容

为解决现有技术存在的问题，本发明提出一种水下单目亚像素相对位姿估计方法，针对水下对接作业任务，利用安装在水下机器人上的水下单目视觉系统(单目RGB相机)采集的光源标靶图像，设计PnP位姿估计算法，并进行迭代优化，实时输出水下机器人相对于目标对接装置的位姿信息，该方法可以准确的得到相对位姿信息，完成水下机器人自主对接工作。

技术方案

本发明的技术原理是：

利用水下单目视觉系统得到目标光源标靶图像中特征点光源的中心点在像素坐标系下的二维坐标，并与其在世界坐标系下的坐标作为输入，采用共面PnP算法和RLS优化算法，对单目视觉系统与目标光源标靶间的相对位姿进行求解。

具体步骤如下：

步骤1：在目标对接装置上设置光源标靶，光源标靶上具有至少5个特征光源，而且其中3个特征光源中心处于同一直线上，事先获取特征光源中在某一设定的世界坐标系下的三维坐标；

步骤2：利用水下单目视觉系统采集光源标靶图像，获取像素坐标系下，图像中每个特征点的坐标；

步骤3：以特征点在世界坐标系下的坐标和像素坐标系下的坐标作为输入，并利用水下单目视觉系统相机的内参数矩阵，采用共面PnP算法，得到世界坐标系相对于相机坐标系的相对位姿矩阵^cM_w的粗值；

步骤4：以得到的^cM_w粗值为初始值，采用RLS优化算法，迭代得到^cM_w精确值。