CN108765489A - 一种基于组合靶标的位姿计算方法、系统、介质及设备 - Google Patents

Abstract

本发明涉及一种基于组合靶标的位姿计算方法、系统、介质及设备，其中的方法包括：通过固定在机械臂末端执行器上的视觉相机拍摄圆形目标和组合靶标的图像，从图像中提取分别提取圆形目标特征和靶标特征，结合预先标定的圆形目标和靶标的空间位置，计算视觉相机相对于圆形目标的概略位姿，利用最小二乘法对概略位姿进行修正，最后确定机械臂末端执行器相对于圆形目标的位姿。本发明利用组合靶标解算机械臂末端执行器与目标之间的相对位姿关系，可为机械臂与目标之间的运动规划提供精确引导。

Description

一种基于组合靶标的位姿计算方法、系统、介质及设备

技术领域

本发明涉及视觉定位领域，具体涉及一种基于组合靶标的位姿计算方法、系统、介质及设备。

背景技术

地外天体表面采样与返回是世界航天大国探索深空和拓展人类认知外星球和太阳系的重要手段。现有的基于地面遥操作模式实现地外天体采样的过程中，因无法估计机械臂末端与目标之间位姿，导致地面遥操作人员难以有效控制机械臂操作。

发明内容

本发明针对上述技术问题，提供一种基于组合靶标的位姿计算方法、系统、介质及设备。

本发明解决上述问题的技术方案：一种基于组合靶标的位姿计算方法，包括：

通过固定在机械臂末端执行器上的视觉相机拍摄圆形目标、圆形靶标和棋盘格靶标的图像；

从所述图像中分别确定所述圆形目标的第一中心点和所述圆形靶标的第二中心点在图像坐标系下的第一坐标和第二坐标；

根据所述第一坐标和获取到的所述第二中心点在世界坐标系下的第四坐标，建立所述第二坐标和第四坐标的对应关系；

根据所述对应关系，计算所述视觉相机相对于所述圆形目标的第一位姿；

根据所述第一位姿和获取到的所述棋盘格靶标的控制点在世界坐标系下的第五坐标，按照透视投影原理，分别确定所述第二中心点和控制点在图像坐标系下的第六坐标和第七坐标；

从所述图像中确定所述控制点在图像坐标系下的第三坐标；

计算所述第二坐标和第六坐标、以及第三坐标和第七坐标之间的偏移误差，并根据所述偏移误差，利用最小二乘法对所述第一位姿进行修正；

根据所述经过修正的第一位姿和机械臂手眼标定关系，确定所述机械臂末端执行器相对于所述圆形目标的第二位姿。

本发明还提供一种基于组合靶标的位姿计算系统，包括：

图像采集模块，用于通过固定在机械臂末端执行器上的视觉相机拍摄圆形目标、圆形靶标和棋盘格靶标的图像；

第一坐标计算模块，用于从所述图像中分别确定所述圆形目标的第一中心点、所述圆形靶标的第二中心点和所述棋盘格靶标的控制点在图像坐标系下的第一坐标、第二坐标和第三坐标；

关系建立模块，用于根据所述第一坐标和获取到的所述第二中心点在世界坐标系下的第四坐标，建立所述第二坐标和第四坐标的对应关系；

第一位姿计算模块，用于根据所述对应关系，计算所述视觉相机相对于所述圆形目标的第一位姿；

第二坐标计算模块，用于根据所述第一位姿和获取到的所述棋盘格靶标的控制点在世界坐标系下的第五坐标，按照透视投影原理，分别确定所述第二中心点和控制点在图像坐标系下的第六坐标和第七坐标；

位姿修正模块，用于计算所述第二坐标和第六坐标、以及第三坐标和第七坐标之间的偏移误差，并根据所述偏移误差，利用最小二乘法对所述第一位姿进行修正；

第二位姿计算模块，用于根据所述经过修正的第一位姿和机械臂手眼标定关系，确定所述机械臂末端执行器相对于所述圆形目标的第二位姿。

本发明实施例还提供一种计算机可读存储介质，包括指令，当所述指令在计算机上运行时，使所述计算机执行上述方法。

本发明还提供一种计算机设备，包括存储器、处理器及存储在所述存储器上的并可在所述处理器上运行的计算机程序，所述处理器执行所述程序时实现上述方法。

本发明利用机械臂末端执行器上所安装的视觉相机获取靶标和目标的图像，基于人工设定的棋盘格靶标和拍摄场景中的自然特征靶标(即圆形靶标)，并配合靶标的先验信息，可以解算机械臂末端执行器相对于目标的位置和姿态，从而引导机械臂调整自身位置以逐步逼近目标位置，实现地外天体表面采样之后将样品准确放入目标过程中的无人化操作。

附图说明

图1是基于组合靶标的机械臂末端位姿计算方法流程图；

图2是机械臂末端执行器、视觉相机、棋盘格靶标、圆形靶标和圆形目标的结构示意图；

图3是圆形靶标编号对应关系示意图；

图4是圆形靶标在图像中投影过程示意图；

图5是对图像中边缘点构成的曲线进行抽样和分割点求取辅助图；

图6是弧段中的点到拟合椭圆距离示意图；

图7是对图像中多个弧段拟合椭圆的合并示意图；

图8是预设角点模板示意图；

图9是检测到的棋盘格靶标角点类型示意图。

具体实施方式

以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。

图1给出了本发明实施例提供的一种基于组合靶标的位姿计算方法的示意性流程图，如图1所示，该方法包括：

S101、通过固定在机械臂末端执行器上的视觉相机拍摄圆形目标、圆形靶标和棋盘格靶标的图像；

图2为机械臂、执行器、视觉相机、圆形靶标、棋盘格靶标和圆形目标的结构示意图，组合靶标即为圆形靶标和棋盘格靶标，圆形目标为机械臂放置样品的目标罐的圆形罐口。其中，执行器固定在机械臂末端，视觉相机固定在执行器上，圆形靶标、棋盘格靶标和目标罐固定于视觉相机的拍摄范围内，其中圆形靶标由布设在目标一侧的自然特征圆组成，圆心作为靶标控制点，圆心间距精确已知。棋盘格靶标是指靶标平面上布设的黑白相间的棋盘格，黑方块和白方块公有的内顶点称为棋盘格靶标的控制点。

S102、从所述图像中分别确定所述圆形目标的第一中心点和所述圆形靶标的第二中心点在图像坐标系下的第一坐标和第二坐标；

该步骤的具体过程是，提取所述图像中的轮廓，通过弧段筛选算法从所述轮廓中剔除无效弧段，选择有效弧段，根据弧段长度和闭合程度对有效弧段进行分类，即如果某一弧段长度满足一定阈值，且弧段上起始点和终止点距离差在设定的阈值区间，则该有效弧段属于圆形靶标有效弧段，其余弧段属于圆形目标有效弧段，利用最小二乘算法对每个有效弧段进行椭圆拟合，并对拟合出椭圆的准确性进行检测，合并交叠椭圆、剔除无效椭圆，得到最终正确的椭圆，根据最终得到的椭圆的方程即可确定圆形目标中心点和圆形靶标中心点在图像坐标系下的坐标。

S103、根据所述第一坐标和获取到的所述第二中心点在世界坐标系下的第四坐标，建立所述第二坐标和第四坐标的对应关系；

在该步骤中，圆形靶标的中心点和棋盘格靶标的控制点在系统初始化之前已经过标定，即其在世界坐标系下的坐标已知。在图像上检测到圆形靶标的中心点之后，要建立圆形靶标的中心点在图像坐标系下的坐标和世界坐标系下的坐标之间的对应关系，作为下一步目标定位计算的先验信息。本实施例设定圆形靶标数量为3，且靶标间相对位置关系如图3所示，结合图3建立圆形靶标的中心点在图像坐标系下的坐标和世界坐标系下的坐标之间的对应关系。

将3个圆形靶标编号，分别为第1、2和3号圆，其在图像中的投影椭圆对应同样的编号值，同时定义0号椭圆为拟合出的圆形目标椭圆。首先，由于圆形目标的半径大于圆形靶标，因此，可以根据拟合出椭圆的长轴大小可以判断出0号椭圆中心点位置坐标，即若满足a_j＝max(a_i)(i,j＝0,1,2,3)，则p₀(x₀,y₀)＝p_j(x_j,y_j)，其中，a_i为i号椭圆的长轴，a_j为j号椭圆的长轴，p₀(x₀,y₀)为0号椭圆的中心点位置坐标，p_j(x_j,y_j)为j号椭圆的中心点位置坐标；以0号椭圆中心点为坐标原点，以与图像x轴平行方向定义为坐标系起始轴，逆时针旋转方向为正，顺时针旋转方向为负，旋转角度范围为[-2π,2π]。θ_ij(1≤i≤3，1≤j≤3且i≠j)表示向量V_0i与V_0j之间的夹角，夹角θ_ij可以根据向量内积公式计算得到。根据椭圆之间的位置关系可知，当θ₁₂、θ₁₃和θ₂₃都为正值时，即按逆时针方向旋转时，θ₁₃＞θ₁₂且θ₁₃＞θ₂₃，因此如果能够找到θ_ij中的最大值，即max(θ_ij)(i,j＝1,2,3)，则可以判断出2号椭圆的位置。根据1、2、3号椭圆之间的位置关系可知，θ_i2＞0且θ_j2＜0，则可进一步判断出p_i(x_i,y_i)是1号椭圆中心位置，p_j(x_j,y_j)是3号椭圆中心位置。

S104、根据所述对应关系，计算所述视觉相机相对于所述圆形目标的第一位姿；

设圆形靶标中心点在图像坐标系中的坐标分别为p₁(x₁,y₁)，p₂(x₂,y₂)，p₃(x₃,y₃)，三个圆形靶标中心点之间的距离分别为d₁，d₂，d₃，距离相机中心的距离分别L₁，L₂，L₃，到相机中心连线之间的夹角分别为α，β和γ，结合图4对第一位姿的计算过程进行分析如下：

根据余弦定律，得到：

令L₁＝λ₁L₃，L₂＝λ₂L₃，其中λ₁、λ₂为比例系数。代入式(1)后抵消掉L₃和λ₂可得关于λ₁的四次多项式：

其中：

式(3)中，d₁，d₂，d₃是已知的，cosα，cosβ，cosγ由余弦定律计算得到。以cosα为例，其计算过程如下：

其中f为视觉相机的焦距。

将式(3)的计算结果代入式(2)解四次方程组，可以得到λ₁的解，进而得到L₃，L₁，λ₂和L₂，即：

进而得到圆形靶标中心点V_i(i＝1,2,3)在相机坐标系中的坐标H_i ^c为：

定义世界坐标系的原点在圆形目标中心点，圆形靶标中心点V_i在世界坐标系下的坐标在系统初始状态下已标定为已知量，用来表示，则世界坐标系和相机坐标系之间的转换关系用下式表示：

其中，R(θ^cw)表示从世界坐标系到相机坐标系的旋转矩阵，t^cw为平移矩阵，为圆形靶标中心点V_i在相机坐标系中的坐标。将式(4)计算得到的和初始标定的值代入(5)，从而计算得到视觉相机相对于圆形目标的位姿，并根据旋转矩阵和姿态角的变换关系，得到相机相对于圆形目标的位置和姿态角即

S105、根据所述第一位姿和获取到的所述棋盘格靶标的控制点在世界坐标系下的第五坐标，按照透视投影原理，分别确定所述第二中心点和控制点在图像坐标系下的第六坐标和第七坐标；

由于圆形靶标中心点和棋盘格靶标控制点在世界坐标系下的坐标已事先标定，根据视觉相机成像中的透视投影原理，可以反推得到圆形靶标中心点和棋盘格靶标控制点在图像坐标系下的坐标，即第六坐标和第七坐标，具体过程如下：

本实施例选定的棋盘格靶标控制点数为6个，圆形靶标中心点数为3个，棋盘格靶标控制点和圆形靶标中心点(以下合称为靶标控制点)在相机成像平面中成像满足透视投影原理，设u_j为第j个靶标控制点在图像中投影坐标，其中，j＝1,…9，则满足以下公式：

其中，M^P∈R^3×3表示从靶标控制点到图像坐标点的透视投影变换矩阵，M^δ∈R^3×3表示与投影坐标相关的畸变参数矩阵，定义如下：

其中是第j个靶标控制点在相机坐标系下z坐标值，f_x和f_y为以像素为单位的相机焦距，(u₀,v₀)为图像主点坐标，Δu_i和Δv_i表示由于径向畸变、偏心畸变和薄透镜畸变引起的投影点偏移量。

R(θ^cw)和t^cw分别表示从世界坐标系到相机坐标系的旋转矩阵和平移矩阵，分别表示第j个靶标控制点在相机坐标系和世界坐标系中的齐次坐标。

将9个靶标控制点的在空间坐标系下的坐标(第四坐标和第五坐标)，即代入式(6)，计算得到靶标控制点在图像上投影坐标位置(第六坐标和第七坐标)，即u_j＝(u_j,v_j)，j＝1,…9。

S106、从所述图像中确定所述控制点在图像坐标系下的第三坐标；

在该步骤中，首先将预设角点模板与所述图像卷积，计算图像上每个像素点的角点相似度值，利用非极大值抑制法抑制角点相似度值小于一定阈值的像素点，得到候选角点；然后基于梯度方向直方图对所述候选角点进行筛选，确定准确的棋盘格靶标角点；最后根据角点边缘和梯度垂直的特性确定棋盘格靶标角点的亚像素位置坐标，将与一个棋盘格靶标控制点的第七坐标的误差值最小的亚像素位置坐标作为该棋盘格靶标控制点的第三坐标。

S107、计算所述第二坐标和第六坐标、以及第三坐标和第七坐标之间的偏移误差，并根据所述偏移误差，利用最小二乘法对所述第一位姿进行修正；

该步骤用于机械臂末端执行器的精确位姿计算，主要通过组合靶标图像投影位置理论值和靶标检测位置之间的偏差误差最小化模型求解机械臂末端执行器与目标罐之间的位姿变换关系。其中靶标图像投影位置理论值是指步骤105中通过透视投影变换的方法计算靶标在相机图像平面中的投影坐标，即第二坐标和第三坐标。靶标检测位置是指通过步骤S103和步骤S106计算得到的圆形靶标中心点和棋盘格靶标控制点在相机图像平面中的第六坐标和第七坐标。将靶标图像投影位置理论值与检测位置进行比较，计算偏移误差，再根据偏移误差利用最小二乘法对步骤S104计算得到的相机位姿结果进行修正。具体实现过程如下所示：

组合靶标图像投影位置理论值与检测位置之间的偏移误差最小化的公式表示为：

公式表示为：

其中为根据透视投影方程计算得到的靶标控制点的图像投影位置理论值，即第六坐标和第七坐标，u_i为从图像中提取的靶标的检测位置，即第二坐标和第三坐标，和分别是视觉相机相对于圆形目标的位置和姿态角，N为靶标控制点的个数，N＝9。

由式(6)可知，为非线性函数，可通过泰勒级数线性化展开如下：

其中表示的雅克比矩阵，即令L＝[L₁,L₂,…,L_N]，根据线性最小二乘法可知公式(8)满足：

根据公式(9)可求得即

迭代执行上述公式，更新和当满足时迭代停止，最终求得的为视觉相机相对于圆形目标的位姿。

S108、根据所述经过修正的第一位姿和机械臂手眼标定关系，确定所述机械臂末端执行器相对于所述圆形目标的第二位姿。

机械臂手眼标定关系，即指机械臂末端执行器与圆形目标之间位姿的旋转变换关系，包括末端执行器坐标系相对于相机坐标系的转换关系，以及相机坐标系相对于世界坐标系的转换关系，前者为预先标定已知量，后者由步骤107中得到的经过修正的位姿表达，从而可以得到末端执行器相对于圆形目标的位姿。具体实现过程如下：

令分别表示末端执行器坐标系相对于相机坐标系的位置和姿态，为预先标定已知量。则末端执行器相对于圆形目标的位姿可通过旋转变换矩阵求得。设表示相机坐标系o_cx_cyczc的任一齐次向量，其在末端执行器中的取值为在圆形目标坐标系oxyz中的取值满足下式：

根据式(10)可得机械臂末端执行器与圆形目标之间的相对位置关系：

本发明实施例提供的一种基于组合靶标的位姿计算方法，利用机械臂末端执行器上所安装的视觉相机获取靶标和目标图像，基于人工设定的棋盘格靶标和拍摄场景中的自然特征靶标(即圆形靶标)，并配合靶标的先验信息，可以解算机械臂末端执行器相对于目标的位置和姿态，从而引导机械臂调整自身位置以逐步逼近目标位置，实现地外天体表面采样之后将样品准确放入目标过程中的无人化操作。

可选地，在该实施例中，所述从图像中确定圆形靶标中心点在图像坐标系下的第二坐标，具体包括：

S201、提取所述图像中的轮廓，得到由所述轮廓上的边缘点组成的多个弧段；

该步骤中，提取轮廓可使用多种现有算法实现，提取到轮廓后，即可得到多个由轮廓上的边缘点组成的弧段。

S202、根据预设的弧段长度和闭合程度对所述多个弧段进行筛选，得到圆形靶标弧段；

该步骤是采用弧段筛选算法对步骤S201中得到的弧段进行筛选和编组，舍去无效弧段，选择有效弧段，并从有效弧度中得到圆形靶标弧段集。具体包括如下三个步骤：第一步是边缘跟踪连接与细化，第二步是从连接和细化的弧段中提取圆形靶标的有效轮廓。结合图5对上述弧段筛选算法介绍如下：

第一步，边缘跟踪连接与细化。对于图像中的一个边缘点P_i(x_i,y_i)，以该边缘点为起点在周围8邻域进行边缘点搜寻，将搜寻到的边缘点加入到P_i的边缘序列点中构建曲线。如果周围8邻域搜得的边缘点数量大于2，判断边缘是否有分叉，如果边缘有分叉，则将分叉边缘定义为一个新的边缘曲线，如果边缘无分叉则删除多余的边缘点。

第二步，从连接和细化的弧段中提取圆形靶标的有效轮廓。根据图像上提取出弧段的长度和闭合程度选取圆形靶标的有效轮廓。首先计算每个弧段长度L_j，选择长度在一定阈值区间弧段，即L₁≤L_j≤L₂。之后遍历所选弧段上边缘点，计算弧段上边缘点起始位置P₁和终止位置P_N的距离差M，即M＝||P_N-P₁||，其中N表示该弧段上边缘点个数。当距离差小于给定的阈值时，则判断此弧段满足闭合条件，并将该有效弧段归入圆形靶标弧段集中。

S203、对筛选得到的每个所述圆形靶标弧段，分别采用最小二乘法拟合椭圆，得到对应的圆形靶标椭圆方程；

基于弧段筛选算法得到多个有效弧段，对每个弧段采用最小二乘算法拟合椭圆，求取椭圆方程描述参数的方法描述如下：

椭圆方程可通过带约束的二次曲线进行描述，其一般形式为：

令α＝[A B C D E F]，X＝[x² 2xy y² 2x 2y 1]，F(α,X_i)为弧段上第i个点(x_i,y_i)(i＝1,2,…,N)到式(11)所示椭圆曲线的代数距离，基于代数距离的最小二乘曲线拟合可表示为：

即求使得所有点到椭圆的代数距离平方和达到最小的二次曲线系数求解时为了简化计算，将式(11)中的不等式约束简化为等式约束，即设定约束条件为AC-B²＝1，此约束条件可用矩阵表示为α^THα＝1，其中H表示为：

在上面的约束条件下，式(12)可转换为广义特征值问题进行求解，即：

其中，S＝U^TU，且表示所有像素点集。考虑U、S和H的特殊性，我们进行矩阵分块得：

根据上述分块关系，可重新构建矩阵方程：

S₁α₁+S₂α₂＝λH₁α₁ (14)

由公式(15)可知，当S₃为非奇异矩阵时，α₂可表示为α₁的线性变换，即将其代入公式(14)可得：

由于H₁为非奇异矩阵，公式(11)可写成：

公式(16)中的等式约束重写为：

令则椭圆拟合求解问题转化为求解Mα₁＝λα₁的问题，即求取矩阵M的特征值的问题。考虑到α₁H₁α₁＝1对特征值非负性的约束，取M的非负特征值对应的特征向量作为α₁的解。根据求得α₂，从而得到椭圆曲线一般方程的系数α，即得到二次曲线椭圆方程的描述参数A,B,C,D,E,F。

S204、将所述圆形靶标弧段上的每个边缘点代入所述对应的圆形靶标椭圆方程，计算残差；

S205、将残差超过预设阈值的边缘点剔除，对剩余的边缘点再次进行椭圆拟合，直到所述圆形靶标弧段上的边缘点的均方差小于预设阈值；

为了抑制图像噪声的影响，提高定位精度，可对边界进行多次拟合。即第一次拟合后，将每个边界点代入上式，计算残差。然后将残差较大的一部分点剔除，再对剩余的点进行二次椭圆拟合。该过程可以重复若干次，直到均方差小于某一阈值为止。

基于上述的二次曲线椭圆方程的描述参数，可以求解椭圆方程的标准形式为：

其中，(x₀,y₀)为椭圆中心坐标，a为椭圆的长轴，b为短轴，θ是长轴和图像坐标系中x轴的夹角。利用二次曲线椭圆方程的描述参数A,B,C,D,E,F计算出这5个参数的计算过程如式(12)所示，其中尺度因子F＝1：

椭圆的长、短轴a和b可根据式(18)求出如下：

其中，A′、C′、D′、E′由式(19)计算得到：

S206、分别计算剩余的各个边缘点到对应的拟合椭圆的距离；

S207、剔除距离大于预设阈值的边缘点，对剩余的边缘点再次进行椭圆拟合；

S208、计算所有边缘点到再次拟合的椭圆的距离和；

S209、当确定所述距离和小于预设阈值时，将每个所述圆形靶标最后拟合的椭圆的中心点的坐标作为所述第二坐标。

步骤S206至S209的具体实现过程如下：

对拟合出的靶标椭圆，在得到椭圆中心坐标(x₀,y₀)、θ和长、短轴a、b后，可利用轮廓上的点到拟合出椭圆的距离来对拟合出椭圆的准确性进行判断。判断过程结合图6介绍如下：

首先定义椭圆坐标系，坐标系原点定义为拟合椭圆的中心，坐标系起始轴定义为与椭圆长半轴平行方向，则图像坐标系到椭圆坐标系的转换关系为

u＝R(x-x₀)

其中在椭圆坐标系中，椭圆方程可以用下式表示：

对于轮廓上的点u_i，其在椭圆上投影点u_i1满足以下方程：

f₂(u,v)＝b²u(v_i-v)-a²v(u_i-u)＝0

u_i1可以利用广义牛顿法来获取，即：

其中，雅克比矩阵

设u_i0＝0.5(u_i2+u_i3)，其中

通过式(20)可以计算得到u_i1的值。对于轮廓上的点x_i，其到椭圆的距离可用式D(x_i)＝||x_i-x_i1||表示，其中x_i1表示点x_i在椭圆上投影点坐标。根据式(20)的转换关系，可得：

D(x_i)＝||x_i-x_i1||＝||u_i-u_i1|| (21)

根据式(21)计算轮廓上的点到拟合椭圆的距离，对每个轮廓上的点，计算其到拟合椭圆的距离D(x_i)，剔除距离大于阈值D_max的点，对剩余的点重新进行拟合椭圆，并计算所有点到椭圆距离和，即当M_j小于一定阈值M_max时，此时拟合的椭圆即为最终的圆形靶标椭圆。

可选地，在该实施例中，所述从图像中确定圆形目标中心点在图像坐标系下的第一坐标，具体包括：

S301、从筛选后剩余的弧段中提取圆形目标弧段；

该步骤是在步骤202从有效弧段中提取圆形目标弧段后，对剩余弧段进行抽样和分割，并剔除其中无效弧段，从而得到圆形目标弧段，具体过程是：首先对离散点组成的弧段以固定的弧长ΔS(可令ΔS＝3)进行抽样，得到新的离散点弧段，如图5所示。然后对离散点弧段进行分割点搜寻分割弧段，方法如下：如果相邻两点连线的角度差大于给定阈值δ_c，即|θ₁-θ₂|＞δ_c，或者相邻两点连线的斜率单调变化，即(k₂-k₁)(k₃-k₂)＜0，以坐标形式表示为(Δx₁₂Δy₂₃-Δy₁₂Δx₂₃)(Δx₂₃Δy₃₄-Δy₂₃Δx₃₄)＜0，则在P₃位置对弧段进行分割。其中，Δx_ij＝x_j-x_i，Δy_ij＝y_j-y_i，其中P_i(x_i,y_i)表示离散点的坐标，θ₁表示P₁P₂与P₂P₃的夹角，θ₂表示P₂P₃与P₃P₄的夹角，k₁、k₂、k₃分别表示直线P₁P₂、P₂P₃、P₃P₄的斜率，即对应的角度范围。最后计算每个弧段长度L_i和曲率变化量κ_i，即κ_i＝Δθ_i/L_i。剔除长度小于L_min的弧段和近似直线的弧段，即L_i＜L_min或者κ_i＜ε(ε为一微小量)的弧段。

S302、对每个所述圆形目标弧段，分别采用最小二乘法拟合椭圆，得到对应的圆形目标椭圆方程；

S303、将所述圆形目标弧段上的每个边缘点代入所述对应的圆形目标椭圆方程，计算残差；

S304、将残差超过预设阈值的边缘点剔除，对剩余的边缘点再次进行椭圆拟合，直到所述圆形目标弧段上的边缘点的均方差小于预设阈值；

S305、分别计算剩余的各个边缘点到对应的拟合椭圆的距离；

S306、剔除距离大于预设阈值的边缘点，对剩余的边缘点再次进行椭圆拟合，直到所有边缘点到对应的再次拟合的椭圆的距离和小于预设阈值；

步骤S302至S307中利用弧段拟合椭圆的过程与步骤S203至S209的过程相同，这里不再赘述。

S307、将最后拟合的椭圆方程中，参数向量间距离不大于预设阈值，并且对应的圆形目标弧段的旋转角度范围的交集不为空集的椭圆方程对应的圆形目标弧段合并为同一组；

S308、将同一组中旋转角度范围最大的圆形目标弧段对应的最后拟合的椭圆的中心点的坐标，作为所述第一坐标。

对拟合出的圆形目标椭圆，在得到拟合椭圆参数后，可利用多个有效弧段各自拟合出的椭圆的相似关系，合并同一椭圆的弧段为一弧段组。合并方法结合图7介绍如下：每个有效弧段定义旋转坐标系，坐标系原点定义为拟合椭圆的中心，坐标系起始轴定义为与图像x轴平行方向，逆时针旋转方向为正，顺时针旋转方向为负，旋转角度范围为[-2π,2π]。图7中的三个弧段i、j、k对应的旋转角度范围分别为δ_i[α_i1,α_i2]、δ_j[α_j1,α_j2]、δ_k[α_k1,α_k2]，对应的拟合椭圆参数分别为Ω_i[x_i,y_i,a_i,b_i,θ_i]、Ω_j[x_j,y_j,a_j,b_j,θ_j]、Ω_k[x_k,y_k,a_k,b_k,θ_k]。如果满足||Ω_H-Ω_G||₂≤ξ_Ω且其中H,G∈{i,j,k}，ξ_Ω为给定的阈值，则将弧段H和弧段G合并为同一组。选择弧段组内旋转角度范围最大的弧段的拟合椭圆作为最终的圆形目标椭圆。

可选地，在该实施例中，所述从图像中确定棋盘格靶标控制点在图像坐标系下的第三坐标，具体包括：

S401、计算所述图像中每个像素点与预设角点模版的角点相似度值；

在图像角点检测过程中，相机拍摄的黑白棋盘格靶标的位置是任意的，为了将图像中任意放置的棋盘格角点都检测出来，本发明方法定义了两种不同的角点模板：一种是与坐标轴平行的角点模板，另一种是旋转45°的角点模板，如图8(a)和(b)所示。根据实践经验，使用上述两种角点模板同时对棋盘格靶标的角点进行检测，对于在图像中任意位置放置的棋盘格而言，其角点都能被检测出来。

每种角点模板由4个滤波核组成，分别用A、B、C、D来表示。以与坐标轴平行的角点模板为例，其生成过程如下：对于角点模板上任一位置I_i[u_i,v_i]，通过其与模板中心位置I_m[u_m,v_m]连线生成的向量在模板中心法线方向的投影值t₁,t₂来判断并生成该模板的滤波核，即：

其中，对于与坐标轴平行的模板而言，α₁＝0，α₂＝π/2,对于旋转45度的模板，其值是π/4和-π/4。当满足t₁＜＝-0.1，且t₂＜＝-0.1，则生成与坐标轴平行模板的A滤波核，且将模板该位置的值设为

角点模版建立后，基于角点模板计算图像上每个像素点与模板的相似程度，得到角点相似度值。其计算过程如下：

首先用像素点(i,j)的图像灰度值I(i,j)分别与角点模板n(n＝1,2)的四种滤波核卷积，获得模板n中A、B、C、D四个卷积核在图像每个像素点上的卷积响应值，分别用来表示，即：

对于一个理想角点来说，的响应值应该大于响应值的平均，与此同时的响应值应该很小，对于翻转的角点正好相反。因此，图像上每个像素点与预设模板的相关性强度根据下述公式确定：

其中，表示模板n的两种可能的翻转，如图9所示，即棋盘格角点可能出现在左对角线对黑，右对角线为白；或者左对角线为白，右对角线为黑的情况。其中可以检测出左对角线对黑、右对角线为白且水平放置的棋盘格角点，可以检测出左对角线对黑、右对角线为白且倾斜放置的棋盘格角点，可以检测出左对角线对白、右对角线为黑且水平放置的棋盘格角点，可以检测出左对角线对白、右对角线为黑且倾斜放置的棋盘格角点。这样使用上述四种滤波核同时对棋盘格角点进行检测，可以对棋盘格放置在图像中的任意位置都能够把其中的角点检测出来。

角点相似度值通过取角点原型和翻转计算组合的最大值来表示，利用式(22)可以计算出图像中每个像素点的角点相似度值。

S402、利用非极大值抑制法抑制角点相似度值小于预设阈值的像素点，得到候选角点；

该步骤中，利用非极大值抑制法在各个像素点的4*4邻域内进行非极大值抑制，选取该邻域内角点相似度值最大且最大值大于0.025的极大值像素点，这些点即候选角点。

S403、建立所述候选角点的梯度方向直方图，并基于所述梯度方向直方图对所述候选角点进行筛选；

本发明用梯度统计的方法在一个局部邻域内验证这些候选角点是否是准确角点。具体检验过程如下：

首先建立以候选角点I_m(i_m,j_m)为中心，大小为K×K的窗口邻域，本实施例采用大小为21×21的窗口邻域，对窗口内灰度图像进行sobel滤波，计算图像上每个像素点x方向上的梯度img_du与y方向上的梯度img_dv，同时计算梯度角angle和梯度幅值weight。其中

angle＝atan2(img_dv,img_du)，weight＝sqrt(img_du.^2+img_dv.^2)。

将窗口内所有的梯度方向角映射到一个32bins的直方图中，用梯度幅值作为加权值，用mean-shift算法找到直方图中两个最大峰值的位置，即对应的两个最大梯度。通常角点的直方图有两个幅值相当的峰值，若两个峰值差别太大，则认为该角点是伪角点。根据该原则对候选角点进行初判，过滤掉部分伪角点，得到更接近准确角点的候选角点，计算方式如下：

Δα＝|α₁-α₂|

其中，α₁,α₂分别对应梯度方向直方图两个峰值处对应的梯度角，如果Δα＜T_α，则S402检测到的候选角点为准确角点，否则为伪角点。本实施例设定T_α＝0.3。

去掉伪角点后，根据剩余的候选角点主梯度方向创建角点模板T′，T′的创建过程和前述角点模版的创建过程相似，计算候选角点的期望梯度模板相似度值d，并根据期望梯度模板相似度值和S401中角点相似度值的乘积，利用阈值法得到最终准确角点。其计算过程如下式所示：

其中，f_TA、f_TB、f_TC、f_TD分别表示模板T′中四个卷积核在候选角点邻域内的卷积响应值。

将期望梯度模板相似度值d和角点相似度c的乘积作为角点可能性的判断值，并用阈值进行判断得到最终准确角点，即如果m＝d*c＜T_m，则在候选角点集中删除该伪角点。本实施例设定T_m＝0.01。

S404、根据角点的边缘和梯度垂直的特性，确定经过筛选的候选角点的亚像素位置坐标，将与一个控制点的第七坐标的误差值最小的所述亚像素位置坐标作为该控制点的第三坐标。

在真实图像中，像素灰度值的变化是一个渐变的过程，角点可能出现在两个像素点之间且不是单位像素点的位置，若以相近像素点表示该角点则会产生误差，因此需要确定棋盘格靶标角点的亚像素位置，将角点检测的精度由像素级提高至亚像素级，提高角点定位的精度，同时提高后继位姿解算的精度。

本发明基于角点边缘和梯度垂直的特性确定亚像素位置，具体实现过程是：假设C是一个理想的角点位置，P是C的局部邻域N_I(c′)内的一个像素点，g_p是P点的图像梯度向量，则有g_p·(p-c)＝0。由此，最理想的角点C的位置就是在角点候选点C′的领域N_I(c′)内找到满足以下式子的c′：

式(23)右侧对c′求导并令其等于0，可以得到解析解，即角点的亚像素位置坐标：

从上述亚像素位置坐标中查找与第七坐标误差值最小的角点坐标值作为棋盘格靶标控制点的第三坐标，即当a_j＝min(|u_j-v_i|)且a_j＜T_min时，则v_i是第j个棋盘格靶标控制点的第三坐标，v_i是第i个亚像素位置坐标，从而建立棋盘格靶标控制点的第七坐标和第五坐标之间的映射关系。

本发明实施例还提供一种基于组合靶标的位姿计算系统，该实施例中各个模块的功能已在上面进行介绍，以下不再赘述。该系统包括：

图像采集模块，用于通过固定在机械臂末端执行器上的视觉相机拍摄圆形目标、圆形靶标和棋盘格靶标的图像；

第一坐标计算模块，用于从所述图像中分别确定所述圆形目标的第一中心点、所述圆形靶标的第二中心点和所述棋盘格靶标的控制点在图像坐标系下的第一坐标、第二坐标和第三坐标；

关系建立模块，用于根据所述第一坐标和获取到的所述第二中心点在世界坐标系下的第四坐标，建立所述第二坐标和第四坐标的对应关系；

第一位姿计算模块，用于根据所述对应关系，计算所述视觉相机相对于所述圆形目标的第一位姿；

第二坐标计算模块，用于根据所述第一位姿和获取到的所述棋盘格靶标的控制点在世界坐标系下的第五坐标，按照透视投影原理，分别确定所述第二中心点和控制点在图像坐标系下的第六坐标和第七坐标；

位姿修正模块，用于计算所述第二坐标和第六坐标、以及第三坐标和第七坐标之间的偏移误差，并根据所述偏移误差，利用最小二乘法对所述第一位姿进行修正；

第二位姿计算模块，用于根据所述经过修正的第一位姿和机械臂手眼标定关系，确定所述机械臂末端执行器相对于所述圆形目标的第二位姿。

可选地，在该实施例中，所述第一坐标计算模块，具体包括：

弧段获取单元，用于提取所述图像中的轮廓，得到由所述轮廓上的边缘点组成的多个弧段；

弧段筛选单元，用于根据预设的弧段长度和闭合程度对所述多个弧段进行筛选，得到圆形靶标弧段；

第一椭圆拟合单元，用于对筛选得到的每个所述圆形靶标弧段，分别采用最小二乘法拟合椭圆，得到对应的圆形靶标椭圆方程；

第一残差计算单元，用于将所述圆形靶标弧段上的每个边缘点代入所述对应的圆形靶标椭圆方程，计算残差；

第二椭圆拟合单元，用于将残差超过预设阈值的边缘点剔除，对剩余的边缘点再次进行椭圆拟合，直到所述圆形靶标弧段上的边缘点的均方差小于预设阈值；

第一距离计算单元，用于分别计算剩余的各个边缘点到对应的拟合椭圆的距离；

第三椭圆拟合单元，用于剔除距离大于预设阈值的边缘点，对剩余的边缘点再次进行椭圆拟合；

第二距离计算单元，用于计算所有边缘点到再次拟合的椭圆的距离和；

第一坐标计算单元，用于当确定所述距离和小于预设阈值时，将每个所述圆形靶标最后拟合的椭圆的中心点的坐标作为所述第二坐标。

可选地，在该实施例中，所述第一坐标计算模块，具体还包括：

弧段提取单元，用于从筛选后剩余的弧段中提取圆形目标弧段；

第三椭圆拟合单元，用于对每个所述圆形目标弧段，分别采用最小二乘法拟合椭圆，得到对应的圆形目标椭圆方程；

第二残差计算单元，用于将所述圆形目标弧段上的每个边缘点代入所述对应的圆形目标椭圆方程，计算残差；

第四椭圆拟合单元，用于将残差超过预设阈值的边缘点剔除，对剩余的边缘点再次进行椭圆拟合，直到所述圆形目标弧段上的边缘点的均方差小于预设阈值；

第三距离计算单元，用于分别计算剩余的各个边缘点到对应的拟合椭圆的距离；

第五椭圆拟合单元，用于剔除距离大于预设阈值的边缘点，对剩余的边缘点再次进行椭圆拟合，直到所有边缘点到对应的再次拟合的椭圆的距离和小于预设阈值；

弧段合并单元，用于将最后拟合的椭圆方程中，参数向量间距离不大于预设阈值，并且对应的圆形目标弧段的旋转角度范围的交集不为空集的椭圆方程对应的圆形目标弧段合并为同一组；

第二坐标计算单元，用于将同一组中旋转角度范围最大的圆形目标弧段对应的最后拟合的椭圆的中心点的坐标，作为所述第一坐标。

可选地，在该实施例中，所述第一坐标计算模块，具体还包括：

相似度计算单元，用于计算所述图像中每个像素点与预设角点模版的角点相似度值；

候选点确定单元，用于利用非极大值抑制法抑制角点相似度值小于预设阈值的像素点，得到候选角点；

角点筛选单元，用于基于梯度方向直方图对所述候选角点进行筛选；

第三坐标计算单元，用于根据角点的边缘和梯度垂直的特性，确定经过筛选的候选角点的亚像素位置坐标，将与一个控制点的第七坐标的误差值最小的所述亚像素位置坐标作为该控制点的第三坐标。

本发明实施例还提供一种计算机可读存储介质，包括指令，当所述指令在计算机上运行时，使所述计算机执行上述方法。

本发明实施例还提供一种计算机设备，包括存储器、处理器及存储在所述存储器上的并可在所述处理器上运行的计算机程序，所述处理器执行所述程序时实现上述方法。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于组合靶标的位姿计算方法，其特征在于，包括以下步骤：

通过固定在机械臂末端执行器上的视觉相机拍摄圆形目标、圆形靶标和棋盘格靶标的图像；

从所述图像中分别确定所述圆形目标的第一中心点和所述圆形靶标的第二中心点在图像坐标系下的第一坐标和第二坐标；

根据所述第一坐标和获取到的所述第二中心点在世界坐标系下的第四坐标，建立所述第二坐标和第四坐标的对应关系；

根据所述对应关系，计算所述视觉相机相对于所述圆形目标的第一位姿；

根据所述第一位姿和获取到的所述棋盘格靶标的控制点在世界坐标系下的第五坐标，按照透视投影原理，分别确定所述第二中心点和控制点在图像坐标系下的第六坐标和第七坐标；

从所述图像中确定所述控制点在图像坐标系下的第三坐标；

计算所述第二坐标和第六坐标、以及第三坐标和第七坐标之间的偏移误差，并根据所述偏移误差，利用最小二乘法对所述第一位姿进行修正；

根据所述经过修正的第一位姿和机械臂手眼标定关系，确定所述机械臂末端执行器相对于所述圆形目标的第二位姿。

2.根据权利要求1所述的方法，其特征在于，所述从图像中确定所述圆形靶标的第二中心点在图像坐标系下的第二坐标，具体包括：

提取所述图像中的轮廓，得到由所述轮廓上的边缘点组成的多个弧段；

根据预设的弧段长度和闭合程度对所述多个弧段进行筛选，得到圆形靶标弧段；

对筛选得到的每个所述圆形靶标弧段，分别采用最小二乘法拟合椭圆，得到对应的圆形靶标椭圆方程；

将所述圆形靶标弧段上的每个边缘点代入所述对应的圆形靶标椭圆方程，计算残差；

将残差超过预设阈值的边缘点剔除，对剩余的边缘点再次进行椭圆拟合，直到所述圆形靶标弧段上的边缘点的均方差小于预设阈值；

分别计算剩余的各个边缘点到对应的拟合椭圆的距离；

剔除距离大于预设阈值的边缘点，对剩余的边缘点再次进行椭圆拟合；

计算所有边缘点到再次拟合的椭圆的距离和；

当确定所述距离和小于预设阈值时，将每个所述圆形靶标最后拟合的椭圆的中心点的坐标作为所述第二坐标。

3.根据权利要求2所述的方法，其特征在于，所述从图像中确定所述圆形目标的第一中心点在图像坐标系下的第一坐标，具体包括：

从筛选后剩余的弧段中提取圆形目标弧段；

对每个所述圆形目标弧段，分别采用最小二乘法拟合椭圆，得到对应的圆形目标椭圆方程；

将所述圆形目标弧段上的每个边缘点代入所述对应的圆形目标椭圆方程，计算残差；

将残差超过预设阈值的边缘点剔除，对剩余的边缘点再次进行椭圆拟合，直到所述圆形目标弧段上的边缘点的均方差小于预设阈值；

分别计算剩余的各个边缘点到对应的拟合椭圆的距离；

剔除距离大于预设阈值的边缘点，对剩余的边缘点再次进行椭圆拟合，直到所有边缘点到对应的再次拟合的椭圆的距离和小于预设阈值；

将最后拟合的椭圆方程中，参数向量间距离不大于预设阈值，并且对应的圆形目标弧段的旋转角度范围的交集不为空集的椭圆方程对应的圆形目标弧段合并为同一组；

将同一组中旋转角度范围最大的圆形目标弧段对应的最后拟合的椭圆的中心点的坐标，作为所述第一坐标。

4.根据权利要求1所述的方法，其特征在于，所述从图像中确定棋盘格靶标的控制点在图像坐标系下的第三坐标，具体包括：

计算所述图像中每个像素点与预设角点模版的角点相似度值；

利用非极大值抑制法抑制角点相似度值小于预设阈值的像素点，得到候选角点；

建立所述候选角点的梯度方向直方图，并基于所述梯度方向直方图对所述候选角点进行筛选；

根据角点的边缘和梯度垂直的特性，确定经过筛选的候选角点的亚像素位置坐标，将与一个控制点的第七坐标的误差值最小的所述亚像素位置坐标作为该控制点的第三坐标。

5.一种基于组合靶标的位姿计算系统，其特征在于，包括：

图像采集模块，用于通过固定在机械臂末端执行器上的视觉相机拍摄圆形目标、圆形靶标和棋盘格靶标的图像；

第一坐标计算模块，用于从所述图像中分别确定所述圆形目标的第一中心点、所述圆形靶标的第二中心点和所述棋盘格靶标的控制点在图像坐标系下的第一坐标、第二坐标和第三坐标；

关系建立模块，用于根据所述第一坐标和获取到的所述第二中心点在世界坐标系下的第四坐标，建立所述第二坐标和第四坐标的对应关系；

第一位姿计算模块，用于根据所述对应关系，计算所述视觉相机相对于所述圆形目标的第一位姿；

第二坐标计算模块，用于根据所述第一位姿和获取到的所述棋盘格靶标的控制点在世界坐标系下的第五坐标，按照透视投影原理，分别确定所述第二中心点和控制点在图像坐标系下的第六坐标和第七坐标；

位姿修正模块，用于计算所述第二坐标和第六坐标、以及第三坐标和第七坐标之间的偏移误差，并根据所述偏移误差，利用最小二乘法对所述第一位姿进行修正；

第二位姿计算模块，用于根据所述经过修正的第一位姿和机械臂手眼标定关系，确定所述机械臂末端执行器相对于所述圆形目标的第二位姿。

6.根据权利要求5所述的系统，其特征在于，所述第一坐标计算模块，具体包括：

弧段获取单元，用于提取所述图像中的轮廓，得到由所述轮廓上的边缘点组成的多个弧段；

弧段筛选单元，用于根据预设的弧段长度和闭合程度对所述多个弧段进行筛选，得到圆形靶标弧段；

第一椭圆拟合单元，用于对筛选得到的每个所述圆形靶标弧段，分别采用最小二乘法拟合椭圆，得到对应的圆形靶标椭圆方程；

第一残差计算单元，用于将所述圆形靶标弧段上的每个边缘点代入所述对应的圆形靶标椭圆方程，计算残差；

第二椭圆拟合单元，用于将残差超过预设阈值的边缘点剔除，对剩余的边缘点再次进行椭圆拟合，直到所述圆形靶标弧段上的边缘点的均方差小于预设阈值；

第一距离计算单元，用于分别计算剩余的各个边缘点到对应的拟合椭圆的距离；

第三椭圆拟合单元，用于剔除距离大于预设阈值的边缘点，对剩余的边缘点再次进行椭圆拟合；

第二距离计算单元，用于计算所有边缘点到再次拟合的椭圆的距离和；

第一坐标计算单元，用于当确定所述距离和小于预设阈值时，将每个所述圆形靶标最后拟合的椭圆的中心点的坐标作为所述第二坐标。

7.根据权利要求6所述的系统，其特征在于，所述第一坐标计算模块，具体还包括：

弧段提取单元，用于从筛选后剩余的弧段中提取圆形目标弧段；

第三椭圆拟合单元，用于对每个所述圆形目标弧段，分别采用最小二乘法拟合椭圆，得到对应的圆形目标椭圆方程；

第二残差计算单元，用于将所述圆形目标弧段上的每个边缘点代入所述对应的圆形目标椭圆方程，计算残差；

第四椭圆拟合单元，用于将残差超过预设阈值的边缘点剔除，对剩余的边缘点再次进行椭圆拟合，直到所述圆形目标弧段上的边缘点的均方差小于预设阈值；

第三距离计算单元，用于分别计算剩余的各个边缘点到对应的拟合椭圆的距离；

第五椭圆拟合单元，用于剔除距离大于预设阈值的边缘点，对剩余的边缘点再次进行椭圆拟合，直到所有边缘点到对应的再次拟合的椭圆的距离和小于预设阈值；

弧段合并单元，用于将最后拟合的椭圆方程中，参数向量间距离不大于预设阈值，并且对应的圆形目标弧段的旋转角度范围的交集不为空集的椭圆方程对应的圆形目标弧段合并为同一组；

第二坐标计算单元，用于将同一组中旋转角度范围最大的圆形目标弧段对应的最后拟合的椭圆的中心点的坐标，作为所述第一坐标。

8.根据权利要求5所述的系统，其特征在于，所述第一坐标计算模块，具体还包括：

相似度计算单元，用于计算所述图像中每个像素点与预设角点模版的角点相似度值；

候选点确定单元，用于利用非极大值抑制法抑制角点相似度值小于预设阈值的像素点，得到候选角点；

角点筛选单元，用于基于梯度方向直方图对所述候选角点进行筛选；

第三坐标计算单元，用于根据角点的边缘和梯度垂直的特性，确定经过筛选的候选角点的亚像素位置坐标，将与一个控制点的第七坐标的误差值最小的所述亚像素位置坐标作为该控制点的第三坐标。

9.一种计算机可读存储介质，包括指令，其特征在于，当所述指令在计算机上运行时，使所述计算机执行根据权利要求1-4任一项所述的方法。

10.一种计算机设备，包括存储器、处理器及存储在所述存储器上的并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-4任一项所述的方法。