一种基于标定物的目标位姿测量方法与流程

本发明属于计算机视觉领域，特别是一种基于标定物的目标位姿测量方法。

背景技术：

计算机视觉是指用相机代替人眼对目标进行观测，结合图像处理技术，从相机拍摄到的图像中提取出所需的关键信息。现在，计算机视觉常被应用于对机械臂的运动控制。机械臂想要抓取某个物体，需要知道目标物体的相关信息，比如目标物体相对于机械臂末端爪具的距离以及目标物体的空间位姿信息。有了这些信息，机械臂才能规划处运动路径以及抓取角度，完成对目标物体及抓取。

在实际应用过程中，经常出现由于目标物体处于复杂背景中，仅仅识别目标物体是比较困难的，相机由于光线等环境因素会出现无法识别目标物体的特征信息或者出现误识别的情况。这对机械臂规划运动路径和选择抓取角度造成里相当大的困难。此外，不同的目标物体可能会存在形状上的差异，这时在目标特征点识别的过程中一些参数的设置也会不同，针对每一个目标物体去手动调节参数是极为不便的。

技术实现要素：

本发明的目的在于提供了一种基于标定物的目标位姿测量方法。

实现本发明的技术解决方案为：一种基于标定物的目标位姿测量方法，具体步骤为：

步骤1、进行相机标定，获得相机的内参数与畸变参数；

步骤2、将标定物贴在被测物体表面，通过相机采集被测物体图像；

步骤3、利用opencv中的cvtcolor函数将步骤2得到的图像转化为灰度图，再利用threshold函数对灰度图进行二值化处理；

步骤4；利用opencv中的findcontours函数，对图像进行找轮廓操作；

步骤5、利用opencv中的approxpolydp函数对步骤4中所有找到轮廓进行多边形逼近，再利用iscontourconvex函数选取其中为凸四边形的轮廓，计算凸四边形最短边长，选取最短边长在规定范围之内的所有轮廓；

步骤6、计算步骤5中四边形轮廓与其他所有轮廓的平均距离，设定阈值为100像素，如果均大于设定的阈值，则将其判定为候选标定物，如果两组轮廓的平均距离小于阈值，则比较二者周长，选取其中周长较大者为标定物；

步骤7、根据标定物的特征对所有候选标定物进行筛查，得到最终的目标标定物；

步骤8、提取标定物的特征点，利用p3p的方法，得到标定物的位姿信息，并用得到的标定物的位姿信息代替目标的位姿信息。

本发明与现有技术相比，其显著优点为：

1)本发明设计了一款标定物，能有效排除环境干扰，使相机能够准确在复杂的背景图像中识别出标定物上特征点，从而计算出相机的位姿信息，准确度和稳定性都比较好，标定物的使用，使相机对目标位姿测量的难度大大的降低，易于实现；2)本发明中测量目标位姿的方法是完全自主式的，不需要额外的人工操作，减轻了工作人员的劳动量，提升了工作效率。

下面结合附图对本发明做进一步阐述。

附图说明

图1为本发明作业流程图。

图2为本发明中设计的标定物。

图3为本发明相机采集到的目标图像。

图4为本发明对图像预处理后的效果图。

图5为本发明对图像二值进行找轮廓操作后的轮廓图。

图6为本发明透视变换效果图。

图7为本发明位姿测量结果。

具体实施方式

一种基于标定物的目标位姿测量方法，具体步骤为：

步骤1、进行相机标定，获得相机的内参数与畸变参数；

步骤2、将标定物贴在被测物体表面，通过相机采集被测物体图像；所述的标定物的物理尺寸为n*n的正方形，并将其分为7*7个相同大小的正方形，其最外围的24正方形全为黑色，除去最外围一圈正方形后，其内部的四个角落上的正方形为3黑1白，其余正方形为黑白相间的正方形。

步骤3、利用opencv中的cvtcolor函数将步骤2得到的图像转化为灰度图，再利用threshold函数对灰度图进行二值化处理；

步骤4；利用opencv中的findcontours函数，对图像进行找轮廓操作；

步骤5、利用opencv中的approxpolydp函数对步骤4中所有找到轮廓进行多边形逼近，再利用iscontourconvex函数选取其中为凸四边形的轮廓，计算凸四边形最短边长，选取最短边长在规定范围之内的所有轮廓；

步骤6、计算步骤5中四边形轮廓与其他所有轮廓的平均距离，设定阈值为100像素，如果均大于设定的阈值，则将其判定为候选标定物，如果两组轮廓的平均距离小于阈值，则比较二者周长，选取其中周长较大者为标定物；

计算轮廓间的平均距离的的方法如下：选取步骤5中得到的两个轮廓，分别记为轮廓a和轮廓b，轮廓a、b的四个角点坐标均已知，分别计算轮廓a四个角点到轮廓b对应的四个角点的距离，求这4组距离的平均值，将其记为两组轮廓的平均距离。

步骤7、根据标定物的特征对所有候选标定物进行筛查，得到最终的目标标定物，具体包括以下步骤：

步骤7-1、检测步骤6中得到的候选标定物，将候选标定物最外围一圈分为24个正方形，分别对这24个正方形的像素值进行统计，统计非零像素值的个数，如果某个小正方形非零像素个数超过正方形总像素个数的一半，则判定该正方形为白色，否则，判定该小正方形为黑色，若这24个正方形内有任何一个正方形为白色，则排除该标定物，最后留下外围24个正方形为黑色的候选标定物；

步骤7-2、对外围正方形为黑色的候选标定物除去外围24个正方形后剩下的内部部分分为25个正方形，分别对这25个正方形的像素值进行统计，统计非零像素值的个数，如果某个小正方形非零像素个数不超过该小正方形总像素个数的一半，则判断该小正方形是黑色。如果黑色小正方形个数为25，则判断该候选标定物内部为全黑，排除该标定物，此时剩下的候选标定物即为所需的标定物。

步骤8、提取标定物的特征点，利用p3p的方法，得到标定物的位姿信息，并用得到的标定物的位姿信息代替目标的位姿信息，具体包括如下步骤：

步骤8-1、将步骤7得到的目标标定物进行透视变换，将其变换到100*100像素的正方形中，透视变换数学表达式如下：

式中(u,v)为原始图像坐标，w的值为1，w′为计算所用的借记量，a11、a12、a21和a22为线性变化参数，a13和a23为透视变化参数，a31和a32为平移参数，a33的值为1，(x,y)为变换之后的图像像素坐标，通过设定的100*100像素小正方形的四个角点与步骤7-2中得到的标定物四个角点的对应关系，利用opencv中getperspectivetransform函数求得透视变换转换矩阵，再利用opencv中的warpperspective函数对图像进行进行透视变换；

步骤8-2、将标定物的4个角点标记为a、b、c、d，计算向量与的叉乘：向量向量则向量的叉乘可以表示为：若叉乘结果为负，则表示ab到ac的旋转是顺时针的，此时需交换b、d两点，若叉乘结果为正，则表示ab到ac的旋转是逆时针的，此时4个角点为逆时针排列；

步骤8-3、检测除去最外围24个全黑正方形后内部4个角落上白色正方形，将其最靠近的角点标记为1号点，此时剩下3个角点逆时针标记为2、3、4号点，标点的具体方法为：

利用透视变换，4个角点经过透视变换后的坐标为(0，0)，(99，0)(99，99)(0，99)，透视变换后，除去最外围黑色正方形后位于角落的白色正方形可能位于左上、右上、右下、左下；

若为左上，则1号点透视变换后的坐标为(0，0)，此时，依次检测4个点中哪个点坐标均在10像素以内，满足这一情况的点为1号点。

若为右上，则1号点透视变换后的坐标为(99，0)，此时，依次检测4个点中哪个点横坐标比纵坐标大50像素以上，满足这一情况的点为1号点；

若为右下，则1号点透视变换后的坐标为(99，99)，此时，依次检测4个点中哪个点坐标均大于50像素，满足这一情况的点为1号点；

若为左下，则1号点透视变换后的坐标为(0，99)，此时，依次检测4个点中哪个点纵坐标比横坐标大50像素以上，满足这一情况的点为1号点。

步骤8-4、根据得到的4组2d角点，同时直接逆时针自定义4组3d初始化，调用opencv中的solvepnpransac函数，得到标定物的位姿信息；

步骤8-5、用得到的标定物的位姿信息代替目标的位姿信息。

一种基于标定物的目标位姿测量系统，包括：

图像处理模块，用于对相机采集到的图像进行图像处理灰度及二值化处理，得到处理后的图像；

检测模块，用于对处理后的图像进行检测，提取图像轮廓后对轮廓进行筛选，匹配，最后得到目标标定物；

位姿计算模块，用于提取标定物的特征点，利用p3p的方法，得到标定物的位姿信息。

一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时以下步骤：

步骤1、进行相机标定，获得相机的内参数与畸变参数；

步骤2、将设计的标定物贴在被测物体表面，通过相机采集被测物体图像；

步骤3、利用opencv中的cvtcolor函数将步骤2得到的图像转化为灰度图，再利用threshold函数对灰度图进行二值化处理；

步骤4；利用opencv中的findcontours函数，对图像进行找轮廓操作；

步骤5、利用opencv中的approxpolydp函数对步骤4中所有找到轮廓进行多边形逼近，再利用iscontourconvex函数选取其中为凸四边形的轮廓，计算凸四边形最短边长，选取最短边长在规定范围之内的所有轮廓；

步骤6、计算步骤5中四边形轮廓与其他所有轮廓的平均距离，如果均大于设定的阈值，则将其判定为候选标定物，如果两组轮廓的平均距离小于阈值，则比较二者周长，选取其中周长较大者为候选标定物；

步骤7、根据设计的标定物的特征对所有候选标定物进行筛查，得到最终的目标标定物；

步骤8、提取标定物的特征点，利用p3p的方法，得到标定物的位姿信息，并用得到的标定物的位姿信息代替目标的位姿信息。

一种计算机可存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：

步骤1、进行相机标定，获得相机的内参数与畸变参数；

步骤2、将设计的标定物贴在被测物体表面，通过相机采集被测物体图像；

步骤3、利用opencv中的cvtcolor函数将步骤2得到的图像转化为灰度图，再利用threshold函数对灰度图进行二值化处理；

步骤4；利用opencv中的findcontours函数，对图像进行找轮廓操作；

步骤5、利用opencv中的approxpolydp函数对步骤4中所有找到轮廓进行多边形逼近，再利用iscontourconvex函数选取其中为凸四边形的轮廓，计算凸四边形最短边长，选取最短边长在规定范围之内的所有轮廓；

步骤6、计算步骤5中四边形轮廓与其他所有轮廓的平均距离，如果均大于设定的阈值，则将其判定为候选标定物，如果两组轮廓的平均距离小于阈值，则比较二者周长，选取其中周长较大者为候选标定物；

步骤7、根据设计的标定物的特征对所有候选标定物进行筛查，得到最终的目标标定物；

步骤8、提取标定物的特征点，利用p3p的方法，得到标定物的位姿信息，并用得到的标定物的位姿信息代替目标的位姿信息。

下面结合附图与实施例对本发明做详细说明

实施例

本发明是一种基于二维标定物的目标位姿测量方法，本发明设计了一种二维标定物，使相机能够准确在复杂的背景图像中识别出二维标定物上特征点，从而得到目标物的位姿信息，方法流程如图1，具体步骤为：

步骤1、进行相机标定，获得相机的内参数与畸变参数，如表1所示：

表1相机标定结果

步骤2、将设计的二维标定物贴在被测物体表面，通过相机采集被测物体图像，所述的二维标定物的物理尺寸为50mm*50mm的正方形，并由7*7个正方形组成，其最外围24个正方形全为黑色，除去最外围一圈正方形后，其内部的四个角落上的正方形为3黑1白，其余正方形为黑白相间的正方形，如图2所示，采集到的图片如图3所示；

步骤3、利用opencv中的cvtcolor函数将步骤2得到的图像转化为灰度图，再利用threshold函数对灰度图进行二值化处理，如图4所示；

步骤4；利用opencv中的findcontours函数，对图像进行找轮廓操作，如图5所示；

步骤5、利用opencv中的approxpolydp函数对步骤4中所有找到轮廓进行多边形逼近，再利用iscontourconvex函数选取其中为凸四边形的轮廓，计算凸四边形最短边长，选取最短边长在规定范围之内的所有轮廓；

步骤6、计算步骤5中四边形轮廓与其他所有轮廓的平均距离，如果均大于设定的阈值，则将其判定为候选二维标定物，如果两组轮廓的平均距离小于阈值，则比较二者周长，选取其中周长较大者为候选二维标定物；

所述计算轮廓间的平均距离的的方法如下：选取步骤5中得到的两个轮廓，分别记为轮廓a和轮廓b，轮廓a、b的四个角点坐标均已知，分别计算轮廓a四个角点到轮廓b对应的四个角点的距离，求这4组距离的平均值，将其记为两组轮廓的平均距离；

步骤7、根据设计的二维标定物的特征对所有候选二维标定物进行筛查，得到最终的目标二维标定物，具体包括以下步骤：

步骤7-1、检测步骤6中得到的候选二维标定物，将候选二维标定物最外围一圈分为24个正方形，分别对这24个正方形的像素值进行统计，统计非零像素值的个数，如果某个小正方形非零像素个数超过正方形总像素个数的一半，则判定该正方形为白色，否则，判定该小正方形为黑色，若这24个正方形内有任何一个正方形为白色，则排除该标定物，最后留下外围24个正方形为黑色的候选二维标定物；

步骤7-2、对外围正方形为黑色的候选二维标定物除去外围24个正方形后剩下的内部部分分为25个正方形，分别对这25个正方形的像素值进行统计，统计非零像素值的个数，如果某个小正方形非零像素个数不超过该小正方形总像素个数的一半，则判断该小正方形是黑色，如果黑色小正方形个数为25，则判断该候选二维标志物内部为全黑，排除该二维标定物，此时剩下的候选二维标定物即为所需的标定物；

步骤8、提取二维标定物的特征点，利用p3p的方法，得到二维标定物的位姿信息，并用得到的二维标定物的位姿信息代替目标的位姿信息，具体包括以下步骤：

步骤8-1、将步骤7得到的目标二维标定物进行透视变换，将其变换到100*100像素的正方形中，透视变换数学表达式如下：

式中(u,v)为原始图像坐标，w的值为1，w′为计算所用的借记量，a11、a12、a21和a22为线性变化参数，a13和a23为透视变化参数，a31和a32，a33的值为1，为平移参数，(x,y)为变换之后的图像像素坐标，通过设定的100*100像素小正方形的四个角点与步骤7-2中得到的二维标定物四个角点的对应关系，利用opencv中getperspectivetransform函数求得透视变换转换矩阵，再利用warpperspective函数对图像进行进行透视变换，透视结果如图6所示；

步骤8-2、将二维标定物的4个角点标记为a、b、c、d，计算向量与的叉乘：向量向量则向量的叉乘可以表示为：若叉乘结果为负，则表示ab到ac的旋转是顺时针的，此时需交换b、d两点，若叉乘结果为正，则表示ab到ac的旋转是逆时针的，此时4个角点为逆时针排列；

步骤8-3、检测除去最外围24个全黑正方形后内部4个角落上白色正方形，将其最靠近的角点标记为1号点，此时剩下3个角点逆时针标记为2、3、4号点，标点的具体方法为：

采用透视变换，得到4个角点经过透视变换后的坐标为(0，0)，(99，0)(99，99)(0，99)，透视变换后，除去最外围黑色正方形后位于角落的白色正方形可能位于左上、右上、右下、左下；

若为左上，则1号点透视变换后的坐标为(0，0)，此时，依次检测4个点中哪个点坐标均在10像素以内，满足这一情况的点为1号点。

若为右上，则1号点透视变换后的坐标为(99，0)，此时，依次检测4个点中哪个点横坐标比纵坐标大50像素以上，满足这一情况的点为1号点；

若为右下，则1号点透视变换后的坐标为(99，99)，此时，依次检测4个点中哪个点坐标均大于50像素，满足这一情况的点为1号点；

若为左下，则1号点透视变换后的坐标为(0，99)，此时，依次检测4个点中哪个点纵坐标比横坐标大50像素以上，满足这一情况的点为1号点；

步骤8-4、根据得到的4组2d角点，直接逆时针自定义4组3d初始化，调用opencv中的solvepnpransac函数，得到二维标定物的位姿信息，如图7所示；

步骤8-5、用得到的二维标定物的位姿信息代替目标的位姿信息。

本方法能有效排除环境干扰，使相机能够准确在复杂的背景图像中识别出标定物上特征点，从而计算出相机的位姿信息，准确度和稳定性都比较好，标定物的使用，使相机对目标位姿测量的难度大大的降低，易于实现。