一种水下单目亚像素相对位姿估计方法技术

本发明专利技术提出一种水下单目亚像素相对位姿估计方法，首先，在目标对接装置上设置光源标靶，光源标靶上具有至少5个特征光源，事先获取特征光源中在世界坐标系下的三维坐标。其次，利用水下单目视觉系统采集光源标靶图像，获取像素坐标系下，图像中每个特征点的坐标。再次，以特征点在世界坐标系下的坐标和像素坐标系下的坐标作为输入，利用水下单目视觉系统相机的内参数矩阵，采用共面PnP算法，得到世界坐标系相对于相机坐标系的相对位姿矩阵

【技术实现步骤摘要】
一种水下单目亚像素相对位姿估计方法


[0001]本专利技术涉及水下机器人视觉
，具体为一种水下单目亚像素相对位姿估计方法。

技术介绍

[0002]机器人远距离长时间自主作业时，因为机器人本身负载和水下通讯限制，需要进行对接作业进入回收装置，进行数据传输和能源补给。长期以来，国内外学者在水下水声信号对接方面取得了很好的研究成果，但由于水声信号只在长距离位置上性能较好，近距离稳定性差、精度低、鲁棒性差，不适用于近距离对接作业中。为了满足自主对接作业的需要，人们需要实现近距离的位姿估计数据的获取，从而控制水下机器人进入对接装置。视觉传感器，以其低成本、高分辨率、高帧率、低噪声、低延迟等优点，在自主对接的相对位姿估计中具有独特的优势，适用于近距离、高精度的对接作业相对位姿估计。
[0003]传统常用的近距离视觉对接作业方法主要采用的是最简单的几何图形学方法来进行导引，一般以光源标靶的中心作为目标导引点，导引水下机器人按照中心点的方向进入对接装置，或者以目标光源点构成的图像面积、周长作为目标，通过其变化之间的关系来导引水下机器人进入对接装置本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种水下单目亚像素相对位姿估计方法，其特征在于：包括以下步骤：步骤1：在目标对接装置上设置光源标靶，光源标靶上具有至少5个特征光源，而且其中3个特征光源中心处于同一直线上，事先获取特征光源中在某一设定的世界坐标系下的三维坐标；步骤2：利用水下单目视觉系统采集光源标靶图像，获取像素坐标系下，图像中每个特征点的坐标；步骤3：以特征点在世界坐标系下的坐标和像素坐标系下的坐标作为输入，并利用水下单目视觉系统相机的内参数矩阵，采用共面PnP算法，得到世界坐标系相对于相机坐标系的相对位姿矩阵
c
M
w
的粗值；步骤4：以得到的
c
M
w
粗值为初始值，采用RLS优化算法，迭代得到
c
M
w
精确值。2.根据权利要求1所述一种水下单目亚像素相对位姿估计方法，其特征在于：步骤1中，采用三维定位系统测量得到每个特征光源中心在世界坐标系中的三维坐标，所述世界坐标系以光源标靶处于左上角位置的特征光源中心为原点。3.根据权利要求1所述一种水下单目亚像素相对位姿估计方法，其特征在于：水下单目视觉系统采用单目RGB相机，安装在水下机器人的下方，保持与水下机器人平行，每帧图像的像素高于800万像素。4.根据权利要求1所述一种水下单目亚像素相对位姿估计方法，其特征在于：步骤1中，水下单目视觉系统采集光源标靶图像后，对图像进行预处理，包括滤波、腐蚀、提取特征光源的光斑边缘轮廓，用于下一步提取特征光源中心在像素坐标系中的坐标。5.根据权利要求1所述一种水下单目亚像素相对位姿估计方法，其特征在于：对于光源标靶图像中的某一特征光源的光斑边缘轮廓，根据公式征光源的光斑边缘轮廓，根据公式确定像素坐标系下的特征光源中心点坐标(a,b)；其中根据特征光源光斑边缘轮廓上的点在像素坐标系下的坐标(u
i
,v
i
)，利用公式解算得到特征光源中心点坐标公式中的符号，E是特征光源光斑边缘轮廓点的集合，上标α和β对应取0,1,2,3。6.根据权利要求1所述一种水下单目亚像素相对位姿估计方法，其特征在于：步骤3中采用共面PnP算法，得到世界坐标系相对于相机坐标系的相对位姿矩阵
c
M
w
的粗值的具体过程为：步骤3
‑
1：建立PnP算法坐标系：已知光源标靶中所有特征点的几何拓扑位置，设第i个特征点在世界坐标系下的坐标
为P
i
＝(x
wi
,y
wi
,z
wi
)，像素坐标系下的坐标为(u
i
,v
i
)，i取1,2,
…
,n；选择光源标靶中的处于同一直线上的三个特征点P1、P2和P3，以上述3个特征点构建参考坐标系；由此得到各个特征点P
i
在参考坐标系下的坐标为P
ri
＝(x
ri
,y
ri
,0)，并且得到参考坐标系相对于世界坐标系下的位姿转换矩阵为
w
M
r
；步骤3
‑
2：求解共面PnP问题：取n个特征点P1～P
n
进行共面PnP问题求解，n不小于4，已知P1～P
n
在世界坐标系下的坐标P
i
＝(x
wi
,y
wi
,z
wi
)和在像素坐标系下的坐标(u
i
,v
i
)；根据水下单目视觉系统的相机内参矩阵，得到特征点P
i
对应在图像坐标系下的坐标针对参考坐标系相对于相机坐标系的位姿转换矩阵得到方程并转换为矩阵形式A1H1+A2H2＝0，其中，＝0，其中，H1＝[
c
η
rxc
η
ryc
η
rz
]
T
H2＝[
c
o
rxc
o
ryc
o
rzc
p
rxc
p
ryc
p
rz
]
T
A1是2n
×
3矩阵，A2是2n
×
6矩阵；构造一个指标函数F＝||A1H1+A2H2||2+λ(1
‑
||H1||2)通过求解
得到H1和H2，使得指标函数F达到最小，其中，λ为任意设定值；根据求得的H1和H2，确定位姿矩阵
c
M
r
的前两列和第四列，
c
M
r
的第三列由第一列和第二列进行叉乘确定；进一步根据公式
c
M
w
＝
c
M
rr
M
w
得到世界坐标系相对于相机坐标系的位姿矩阵
c
M
w
，其中
r
M
w
＝

【专利技术属性】
技术研发人员：高剑，张元旭，张福斌，张立川，陈依民，张飞虎，
申请(专利权)人：西北工业大学，
类型：发明
国别省市：