CN111968180B - 基于参考平面的高精度物体多自由度姿态估计方法及系统 - Google Patents

Abstract

本发明公开了基于参考平面的高精度物体多自由度姿态估计方法及其系统。基于参考平面的高精度物体多自由度姿态估计方法，包括以下步骤：步骤S1：由至少一成像设备实时成像并且实时输出包含待多自由度姿态估计的上述物体的一信息流，上述物体相对于上述参考平面具有有限个稳定的多自由度状态；步骤S2：至少一存储设备被事先预置或者被实时录入上述物体和/或上述成像设备的至少一先验知识信息。本发明公开的基于参考平面的高精度物体多自由度姿态估计方法及系统，试验表明可有效地部署于工业生产环境中，具有较高的可靠性和运行效率。

Description

基于参考平面的高精度物体多自由度姿态估计方法及系统

技术领域

本发明属于智能制造技术领域，具体涉及一种基于参考平面的高精度物体多自由度姿态估计方法和一种基于参考平面的高精度物体多自由度姿态估计系统。

背景技术

在工业领域，若想在现有的自动化水平上，进一步提高设备的柔性生产能力或进一步替代人工操作，配备智能视觉系统将是很难绕开的选择。而且，在智能视觉系统中，对物体多自由度姿态估计，往往是机器人柔性操作的先导条件。在过往的十几年中，已经有了很多多自由度姿态估计的方法，但由于场景的复杂性，物体的多自由度姿态估计仍具有非常大的挑战性。现有的方法大体可分为两类：一类是传统的各种匹配方法，包括模板匹配、描述子匹配、特征匹配等等，其缺点是对光照的变化、物体表面纹理敏感，而且一些精匹配的算法费时费力；另一类是新型的各种基于深度学习的方法，由于深度神经网络的强表示能力，确实改善了对环境变化的适应性，但精度还是很差，很难在精度、效率、可靠性都要求相对高的工业领域实际部署应用。

经长时间的在生产实践中观察发现，物体置于平面上是一种很普遍的场景，或者说，需要对平面上的物体进行6D(六自由度)姿态估计的情况也是个很普遍的情况，而且很多物体在平面上的稳定状态仅有有限的几种，所以若将物体与平面的相对关系，以及摄像头与平面的相对关系的先验知识应用于物体姿态估计，再结合最新的深度学习技术，可以知道，其效果的提升，将在以往的方法还不能有效解决问题的场景中实现应用，实现机器替代人工或提升设备柔性生产能力，所以需要予以进一步改进。

发明内容

本发明针对现有技术的状况，克服以上缺陷，提供一种基于参考平面的高精度物体多自由度姿态估计方法和一种基于参考平面的高精度物体多自由度姿态估计系统。

本发明专利申请公开的基于参考平面的高精度物体多自由度姿态估计方法及系统，其主要目的在于，试验表明可有效地部署于工业生产环境中，特别是汽车零部件生产环境，具有较高的可靠性和运行效率。

本发明专利申请公开的基于参考平面的高精度物体多自由度姿态估计方法及系统，其另一目的在于，除预置的先验知识部分外，在全流程充分体现实时性。

本发明采用以下技术方案，所述基于参考平面的高精度物体多自由度姿态估计方法，用于多自由度地估计一物体，上述物体相对于预设的一参考平面设置，包括以下步骤：

步骤S1：由至少一成像设备实时成像并且实时输出包含待多自由度姿态估计的上述物体的一信息流，上述物体相对于上述参考平面具有有限个稳定的多自由度状态；

步骤S2：至少一存储设备被事先预置或者被实时录入上述物体和/或上述成像设备的至少一先验知识信息；

步骤S3：根据所述先验知识信息，结合预设的一关键点识别方法，实时获取并且输出上述物体相对于一摄像机坐标系的多自由度姿态。

根据上述技术方案，作为上述技术方案的进一步优选技术方案，步骤S1具体实施为以下步骤：由至少一摄像头实时成像并且实时输出包括待多自由度姿态估计的上述物体的至少一图片。

根据上述技术方案，作为上述技术方案的进一步优选技术方案，步骤S2中，所述先验知识信息藉由测量、标定，或向设备厂家索取，或查相应手册方式获得，所述先验知识信息包括但不限于：

摄像头内参数；

待多自由度姿态估计的上述物体的三维数模；

物体在状态i下参考点到参考平面的距离，记为di1,di2,di3,…，i代表第i种稳定状态,参考点可在物体上自由选取；

测量摄像头光轴线与参考平面的夹角，记为θ；

当0<θ<90°时,测量摄像头绕其光轴线旋转的角度α，以摄像头X轴与参考平面平行Y轴正方向在靠近参考平面一侧时α＝0，沿着摄像头的Z轴，右手法则，逆时针方向α为正，顺时针方向α为负，当θ＝90°时，不需要测量α值；

测量摄像头坐标系原点到参考平面的距离，记为hc。

根据上述技术方案，作为上述技术方案的进一步优选技术方案，步骤S3具体实施为以下任一条步骤：实时生成上述物体相对于所述摄像头的一摄像头坐标系的一姿态，或者实时生成上述物体的至少一点相对于所述摄像头的一摄像头坐标系的至少一坐标。

根据上述技术方案，作为上述技术方案的进一步优选技术方案，步骤S3中，所述关键点识别方法，具体实施为以下任一类步骤的集合：

步骤SP3.1：首先对物体进行目标检测，输入的是图像，输出的是目标位置，常用包含目标物的方框表示，用输出的方框crop图像，如果是多个目标，用padding统一尺寸后堆叠在一起形成一个batch送入关键点识别网络；

步骤SP3.2：用diamondnet实现物体关键点识别；

步骤SP3.3：用各种pnp方法粗略估计物体姿态，目的是确定物体相对于参考平面处于哪种状态；

步骤SP3.4：确认当前状态i；或者：

步骤S3中，所述关键点识别方法，具体实施为以下步骤：

步骤SQ3.1：如果待检测的物体种类为一种或几种，且待识别的关键点数较少，藉由diamondnet加上关键点heatmap局部非极大值抑制以及关键点聚类的方法，同时实现关键点识别与物体检测；

步骤SQ3.2：用各种pnp方法粗略估计物体姿态，目的是确定物体相对于参考平面处于哪种状态；

步骤SQ3.3：确认当前状态i；或者：

藉由现有的物体6d姿态估计方法确认物体处于哪种稳定状态i；用关键点识别网络识别出图像上物体的关键点。

根据上述技术方案，作为上述技术方案的进一步优选技术方案，所述关键点识别方法，还包括以下步骤：

步骤SW3.1：藉由以上两阶段信息，计算出坐标轴axis_i上的典型点(0,0,0)，(s,0,0)，(0,s,0)，(0,0,s)，其中，s可取大于0的任意值，在摄像机图像像素坐标系的二维坐标值及摄像机坐标系的三维坐标值；

步骤SW3.2：先求取axis_i坐标原点也就是axis_i坐标下(0,0,0)点在摄像机图像像素坐标系的二维坐标值。

根据上述技术方案，作为上述技术方案的进一步优选技术方案，所述关键点识别方法，还包括以下步骤：

步骤SW3.3：藉由深度神经网络识别出来的，在摄像头获取的图像中参考点对应的非共线的三个关键点的位置，记为(uk,vk)、(um,vm)、(un,vn)，所述三个关键点对应的参考点在参考平面上的垂直投影点记为pki,pmi,pni，由上述的先验知识信息，上述垂直投影点相对于局部坐标系axis_i的坐标为已知；

步骤SW3.4：藉由上述的先验知识信息以及关键点和垂直投影点的坐标信息，获得axis_i坐标系上的(0,0,0)，(s,0,0)，(0,s,0)，(0,0,s)点对应摄像机图像像素坐标系的像素坐标，并求得axis_i坐标系上的(0,0,0)，(s,0,0)，(0,s,0)，(0,0,s)点在摄像机坐标系的坐标，继而采用ransac，ict或直接带入变换方程求解方程组的方法求取axis_i坐标系到摄像机坐标系的变换关系，由于上述axis_i坐标系上的(0,0,0)，(s,0,0)，(0,s,0)，(0,0,s)点对应的摄像机图像像素坐标是基于参考平面获得，所以选择性地再次运用各种pnp方法求取axis_i坐标系到摄像机坐标系的变换关系，再进一步结合物体坐标系到axis_i坐标系的转换关系，获得物体相对于摄像头坐标系的高精度实时姿态。

本发明专利申请还公开了一种基于参考平面的高精度物体多自由度姿态估计系统，用于实施以上任一种技术方案公开的基于参考平面的高精度物体多自由度姿态估计方法的步骤。

本发明专利申请还公开了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如以上任一种技术方案公开的所述基于参考平面的高精度物体多自由度姿态估计方法的步骤。

本发明专利申请还公开了一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现如以上任一种技术方案公开的所述基于参考平面的高精度物体多自由度姿态估计方法的步骤。

本发明公开的基于参考平面的高精度物体多自由度姿态估计方法及系统，其有益效果在于，试验表明可有效地部署于工业生产环境中，具有较高的可靠性和运行效率。

附图说明

图1是本发明的准备阶段的其中一种稳定状态的示意图。

图2是本发明的准备阶段的另一种稳定状态的示意图。

图3是本发明的准备阶段的选取参考点的示意图。

图4是本发明的准备阶段一种稳定状态下的局部坐标系，以及参考点到参考平面距离的示意图。

图5是本发明的准备阶段另一种稳定状态下的局部坐标系，以及参考点到参考平面距离的示意图。

图6是本发明的准备阶段的测量摄像头光轴线与参考平面的夹角，以及摄像机光心(摄像机坐标系原点)到参考平面距离的示意图。

图7是本发明的准备阶段的摄像机轴系(坐标系)的示意图。

图8是本发明的准备阶段的测量α角的示意图。

图9是本发明的物体状态i确定及图像关键点识别阶段的连续三帧图像中的其中一帧图像的示意图。

图10是本发明的物体状态i确定及图像关键点识别阶段的连续三帧图像中的另一帧图像的示意图。

图11是本发明的物体状态i确定及图像关键点识别阶段的连续三帧图像中的再一帧图像的示意图。

图12是本发明的物体状态i确定及图像关键点识别阶段的Diamondnet网络结构细节与一种后处理方法(chanel最大值对应的坐标为chanel对应的关键点坐标)的示意图。

图13是本发明的高精度物体姿态估计阶段，在-种稳定状态下(i＝1时)参考点垂直投影到参考平面，以及求取线段比值γ和β的示意图。

图14是本发明的高精度物体姿态估计阶段，在-种稳定状态下(i＝1时)求取某一参考点(xm,ym,zm)在参考平面的垂直投影点(xm1,ym1,0)在摄像机像素坐标系上的坐标的示意图。

图15是本发明的高精度物体姿态估计阶段的连续三帧图像的其中一帧图像的示意图。

图16是本发明的高精度物体姿态估计阶段的连续三帧图像中的另一帧图像的示意图。

图17是本发明的高精度物体姿态估计阶段的连续三帧图像中的再一帧图像的示意图。

具体实施方式

本发明公开了一种基于参考平面的高精度物体多自由度姿态估计方法，下面结合优选实施例，对本发明的具体实施方式作进一步描述。

值得一提的是，本发明各个实施例可能涉及的“6D”、“6D姿态”、“6个自由度”为同一概念，不再区分。

值得一提的是，本发明各个实施例可能涉及的“摄像头”、“摄像机”为同一概念，不再区分。

值得一提的是，本发明各个实施例可能涉及的“状态”，我们定义为，物体与参考平面的接触状况。

值得一提的是，本发明各个实施例可能涉及的“姿态”，我们定义为，物体相对于摄像机坐标系的具体姿势，包含旋转矩阵R及平移矩阵T或者物体上的每个点在摄像机坐标系上的坐标。

值得一提的是，本发明各个实施例可能涉及的“点”，我们定义为，与物体相关的任意一个点。

值得一提的是，本发明各个实施例可能涉及的“参考点”，我们定义为，人为地随机选择的点，但是优先选择物体局部特征明显的点，例如，角点、圆心点等。

值得一提的是，本发明各个实施例可能涉及的“关键点”，我们定义为，“参考点”或指定点位于成像平面的像素坐标，或者说摄像机图像上对应于“参考点”或指定点的二维像素点。

值得一提的是，本发明各个实施例可能涉及：当物体状态只有一种或由于人为或其他原因物体状态i已知时，就不需要通过pnp相关方法粗略估计姿态确定状态i了。

优选实施例。

优选地，参见附图的图1至图17，所述基于参考平面的高精度物体多自由度姿态估计方法，用于多自由度姿态估计一物体，上述物体相对于预设的一参考平面设置，所述基于参考平面的高精度物体多自由度姿态估计方法包括以下步骤：

步骤S1：由(基于参考平面的高精度物体多自由度姿态估计系统的)至少一成像设备实时成像并且实时输出包含待多自由度姿态估计的上述物体的一信息流，上述(待多自由度姿态估计的)物体相对于上述参考平面具有有限个稳定的多自由度状态；

步骤S2：(基于参考平面的高精度物体多自由度姿态估计系统的)至少一存储设备被事先预置或者(在需要基于参考平面的高精度物体多自由度姿态估计时)被实时录入上述物体和/或上述成像设备的至少一先验知识信息；

步骤S3：根据所述先验知识信息，结合(基于参考平面的高精度物体多自由度姿态估计系统的)预设的一关键点识别方法，(基于参考平面的高精度物体多自由度姿态估计系统)实时获取并且输出上述物体相对于一摄像机坐标系的(具体的)多自由度姿态。

第一实施例。

第一实施例在优选实施例的全部技术方案的基础上，进一步包括以下技术方案。

进一步地，步骤S1具体为以下步骤：由至少一摄像头实时成像并且实时输出包括待多自由度姿态估计的上述物体的至少一图片。

其中，上述物体设置于上述参考平面。

其中，上述物体设置于上述参考平面的过程，确保上述物体位于所述摄像头的视野范围内(即藉由在先的人为设置上述物体的位置，排除上述物体不处于所述摄像头的视野范围内的特例情况；换而言之，所述基于参考平面的高精度物体多自由度姿态估计方法的步骤S1在开始估计上述物体的多自由度姿态时，已默认上述物体处于所述摄像头的视野范围内，不再前置排除上概述物体不处于所述摄像头的特例情况)。

进一步地，为了与步骤S3中的所述关键点识别方法相适配，步骤S2中，所述先验知识信息藉由测量、标定，或向设备厂家索取，或查相应手册方式获得，所述先验知识信息包括但不限于：

摄像头内参数(如果摄像头图像畸变不满足要求，那么还需要畸变参数)；

待多自由度姿态估计的上述物体的三维数模；

物体在状态i下参考点到参考平面的距离，记为di1,di2,di3,…，i代表第i种稳定状态,参考点可在物体上自由选取(也可以选取与物体具有固定位置关系的非物体上的点，如圆心)；测量摄像头光轴线(即摄像头z轴)与参考平面的夹角，记为θ(摄像头的布局要确保θ大于0°小于等于90°)；当0<θ<90°时,测量摄像头绕其光轴线(即绕摄像头z轴)旋转的角度α，以摄像头X轴与参考平面平行Y轴正方向在靠近参考平面一侧时α＝0°，沿着摄像头的Z轴，右手法则，逆时针方向α为正，顺时针方向α为负，当θ＝90°时，不需要测量α值，α可以为任意值；

测量摄像头坐标系原点(也就是镜头光心)到参考平面的距离，记为hc。

进一步地，步骤S3具体实施为以下步骤：实时生成上述物体相对于所述摄像头的一摄像头坐标系的一姿态。

换而言之，步骤S3具体实施为以下步骤：实时生成上述物体的至少一点相对于所述摄像头的一摄像头坐标系的至少一坐标。

第二实施例。

第二实施例在第一实施例的全部技术方案的基础上，进一步包括以下技术方案。

进一步地，所述基于参考平面的高精度物体多自由度姿态估计方法，还包括步骤S0，步骤S0前置于步骤S1：步骤S0：准备阶段步骤，以核实待多自由度姿态估计的(上述)物体是否具有有限个稳定的多自由度状态。具体地，作为举例，确认物体在平面上是否具有有限个稳定状态，或确认物体由于生产工艺或人为原因只会处于一种或几种稳定状态，以附图零件举例：参见附图的图1和图2，在平面上共有2种稳定状态。值得一提的是，如下特殊情况要注意，对于对称物体，物体在平面上的状态要归一化，如对表面纹理朝向无要求的长方体，6个状态归一后为2种状态，如果对表面纹理朝向有要求，根据纹理的对称情况，可归一为大于等于2种小于等于6种的状态；再如对表面纹理朝向无要求的球体，看似不稳定，有无数种状态，但状态归一化后只有一种稳定状态，若对表面纹理朝向有要求，就不适用于本方法了。

进一步地，步骤S0还包括步骤S0.1：步骤S0.1：在(上述)物体任意选取至少3个参考点，以上各个参考点为非共线参考点。

具体地，作为举例，参考点也可以选取与物体有固定位置关系的非物体上的点(一种值得一提的情况是，根据实际需求及状况，在对最终结果的精度要求相对不高时，可选择性地直接选取局部坐标系axis_i上的(0,0,0),(s,0,0),(0,s,0)点作为参考点，axis_i的定义参见下述步骤S0.2,s定义参见下述步骤SW3.1)，但最好选取物体上的点，且选取那些局部特征明显的点，如角点、圆心等，其中要保证至少3个点非共线，也要保证这些点垂直投影到参考平面和摄像头成像平面上至少3个点非共线，获取参考点在物体坐标系中的坐标值，可以藉由已有的数模获取，如果数模与实际物体偏差不满足要求或没有数模，也可以藉由三维扫描获取，记为p1:(x1,y1,z1)，p2:(x2,y2,z2)，p3:(x3,y3,z3)，…(参见附图的图3)。

进一步地，步骤S0还包括步骤S0.2：

步骤S0.2：测量(上述)物体在稳定状态i下参考点到参考平面的距离。

具体地，作为举例，测量物体在稳定状态i下参考点到参考平面的距离，记为di1,di2,di3,…，i代表第i种稳定状态，可取大于零的整数；测量的方法有多种可以拿靠尺量，如果是扫描的方式获取数模的话，可以在扫描数模上藉由相应软件的测量功能量取，如果有数模且数模与实物偏差在允许范围内，可以藉由将数模导入CATIA、UG、PROE等软件中进行测量(参见附图的图4和图5)。

值得一提的是，对每种状态i，建立一个Z轴垂直于参考平面，原点在参考平面上的物体的局部坐标系(有一个轴垂直于参考平面即可，方便起见，选Z轴，正方向取有物体的一面)，记物体坐标系与物体局部坐标系之间的转换关系为Ri、Ti，局部坐标系记为axis_i(下标i代表第i种稳定状态)；要已知摄像机的内参矩阵，若不知可藉由已有的各种摄像机标定法实测获得，或查手册，甚至可以直接向摄像机厂商索取。当物体坐标系与某个物体局部坐标系axis_i重合时，可简化后续运算(参见附图的图4和图5)。

进一步地，步骤S0还包括步骤S0.3：

步骤S0.3：测量摄像头光轴线(即摄像头z轴)与参考平面的夹角。

具体地，作为举例，测量摄像头光轴线(即摄像头z轴)与参考平面的夹角，记为θ，θ大于0°小于等于90°。测量摄像头绕光轴线(即绕摄像头z轴)旋转的角度α，以摄像头X轴与参考平面平行Y轴在靠近参考平面一侧时α＝0°，沿着摄像头的Z轴，右手法则，逆时针方向α为正，顺时针方向α为负，α大于-90°小于等于90°，有一种特殊情况就是,当θ＝90°时，不需要测量α值，α可以为任意值；测量摄像头坐标系原点(也就是镜头光心)到参考平面的距离，记为hc(参见附图的图6、图7和图8)。

值得一提的是，参数的测量方法很多，其中，摄像头安装好后藉由opencv等软件来进行非接触标定测量和矫正是个不错的选择。

值得一提的是，θ＝90°时，是个特殊情况，这时很多处理会变得简便，而且最终结果的精度、可靠性更佳，要优先考虑这种布局来解决实际场景问题；还有，摄像头安装好后，不可能θ正好等于90°，所以还需要借助opencv等软件结合各种标定板(棋盘格、圆形标定板、aruco板等等)进行图像矫正。

值得一提的是，步骤S0中，步骤S0.1、步骤S0.2、步骤S0.3等，本领域技术人员应注意，不应视为默认存在必须顺序执行步骤S0.1、步骤S0.2、步骤S0.3的限定，而应当视为，可以根据实际需要选择性地部分或者全部地顺次顺序/乱序执行步骤S0.1、步骤S0.2和步骤S0.3。

第三实施例。

第三实施例在第二实施例的全部技术方案的基础上，进一步包括以下技术方案。

第三实施例主要用于陈述物体状态i确定及图像关键点识别阶段，即识别参考点在摄像头图像上的实时位置。此阶段有很多实现方法，下面是藉由自设计一种叫diamondnet(在resnet骨架基础上改进得来，识别精度非常高)的网络来实现物体关键点识别，物体关键点识别也可以用公开的hourglass、hrnet等方法。

值得一提的是，以下方法中，目标检测网络及关键点识别网络的训练，都不需要手动标注数据，可以藉由物体的三维模型(已有或扫描获得)，结合blender等渲染软件，自动标记关键点和目标框、也可以生成目标掩码，有很多公开的方法；diamondnet损失函数采用L1smoothLoss，标签采用heatmap(关键点热图)。用diamondnet深度神经网络(也可以用其他深度学习模型如hourglass、hrnet等等)来自动识别出(参考点对应的)关键点的位置，记为(u1,v1),(u2,v2),(u3,v3),…；diamondnet网络结构细节及具体后处理方法，参见Diamondnet示意图(附图的图12)，需要的话，提前对输入图像进行畸变矫正。

值得一提的是，当diamondnet用在方法1时，heatmap每个chanel取最大值，最大值对应的坐标为chanel对应关键点的坐标(相对crop图像)，进一步转换为原输入图像的坐标；当diamondnet用在方法2时，heatmap每个chanel保留非极大值抑制(nms)后的局部极大值大于一定阀值(比如说0.9)的点(参考了cornernet的后处理方法)，局部极大值对应的坐标为chanel对应关键点的坐标，再以物体的中心聚类这些关键点或运用其他聚类方法，以区分哪些关键点具体属于哪个实例，这样就能同时实现物体实例检测和关键点识别。注：本例子中diamondnet是以resnet18作为backbone，当需要识别的物体种类变多时，backbone可以改为resnet34、resnet50或resnet101。

第三实施例的第一种具体实施方式。

具体地，步骤S3中，所述关键点识别方法，具体实施为以下步骤：

步骤SP3.1：首先对物体进行目标检测，输入的是图像，输出的是目标位置，常用包含目标物的方框表示(有很多公开的方法，如yolo系列、centernet等；如果是部分包含目标物时，方框内要确保至少会有3个非共线的关键点)，用输出的方框crop图像，如果是多个目标，用padding统一尺寸后堆叠在一起形成一个batch送入关键点识别网络；步骤SP3.2：用diamondnet(也可以是hourglass、hrnet等)实现物体关键点识别；步骤SP3.3：用各种pnp(epnp，p3p等等)方法粗略估计物体姿态，目的是确定物体相对于参考平面处于哪种状态；步骤SP3.4：确认当前状态i(比如说i＝1)。

第三实施例的第二种具体实施方式。

具体地，步骤S3中，所述关键点识别方法，具体实施为以下步骤：

步骤SQ3.1：如果待检测的物体种类为一种或几种，且待识别的关键点数较少，藉由diamondnet(也可以是hourglass、hrnet等)加上关键点heatmap局部非极大值抑制以及关键点聚类的方法，同时实现关键点识别与物体检测；

步骤SQ3.2：用各种pnp(epnp，p3p等等)方法粗略估计物体姿态，目的是确定物体相对于参考平面处于哪种状态；

步骤SQ3.3：确认当前状态i(比如说i＝1)。

第三实施例的第三种具体实施方式。

具体地，步骤S3中，所述关键点识别方法，具体实施为以下步骤：藉由现有的物体6d姿态估计方法(基于模板，基于RGBD方法等等)确认物体处于哪种稳定状态i；用关键点识别网络(diamondnet、hourglass、hrnet等)识别出图像上物体的关键点。值得一提的是，连续的三帧图像，深色轴系为标准姿态，浅色轴系为仅用pnp算法结合关键点识别得到的姿态，可以发现，估计出的姿态，尤其是z方向拨动很大(参见附图的图9、图10和图11)。值得一提的是，藉由各种pnp算法直接获得的物体姿态，由于实时识别时关键点在小范围震动(关键点识别有偏差，视频输入有扰动且变化(无法避免)，所以偏差拨动不很稳定)，得出的姿态很不稳定，准确率不高，但用来确定状态i是足够的。值得一提的是，试验表明，在本阶段采用第一种或者第二种具体实施方式的情况下，输入分辨率是640*480时整套方案在载有Intel i7 3.7GHz CPU和GTX1080tiGPU(多)计算机上可获得超过25fps速率；实测的关键点平均识别偏差可小于0.6383个像素，而且识别效果相当稳定，具有实用性和推广价值。

第四实施例。

第四实施例在第三实施例的全部技术方案的基础上，进一步包括以下技术方案。第四实施例主要用于陈述高精度物体姿态估计阶段的具体步骤。值得一提的是，高精度物体姿态估计阶段的具体步骤包括步骤SW3.1：(此阶段主要是藉由以上两阶段信息，)计算出坐标轴axis_i(下标i代表第i种稳定状态)上的典型点(0,0,0)、(s,0,0)、(0,s,0)、(0,0,s)(其中，s可取大于0的任意值，在这里暂时取为0.1，单位是米)在摄像机图像上的二维像素坐标值，以及在摄像机坐标系上的三维坐标值(不仅限于典型点，可以是任一点，但典型点有利于确定物体姿态，状态i已在上一阶段中确定)。值得一提的是，高精度物体姿态估计阶段的具体步骤包括步骤SW3.2：先求取axis_i坐标原点也就是axis_i坐标下(0,0,0)点在摄像机图像上的二维像素坐标值，以及摄像机坐标系上的三维坐标值。摄像机图像上的二维目标点的求解方法，不管是用三个参考点还是三个以上参考点，方法都是使待求点在摄像机图像上成为两条线的交叉点或成为线段的比例分割点。这里用三个参考点来求取目标点为例，首先在参考点中任选3个点(状态i下，优先选取从摄像头图像上直接可见的点；对应步骤SP3.1，此时要确保此3个参考点对应的关键点在crop图像范围内；当求取axis_i上典型点(s,0,0)和(0,s,0)的摄像头相关坐标值时也可以选择性地分别再次在参考点中任选3个点)，物体自身坐标系下表示为(xk,yk,zk)、(xm,ym,zm)、(xn,yn,zn)，确保此三点在参考平面上的投影不在一条直线上，也确保在摄像头获取的图片上不在一条直线上。此三点在axis_i坐标系下，在参考平面上的投影点坐标值为(xki,yki,0)、(xmi,ymi,0)、(xni,yni,0)，投影点简记为：pki,pmi,pni(参见附图的图13)，由上述的先验知识信息，此投影坐标为已知,axis_i坐标系原点记为oi，在axis_i坐标系的xy平面内(也就是参考平面内)，直线pni_oi与直线pki_pmi的交点为pti，可以计算出pti的坐标值为： (或/>),确保分母不为0，那么线段pki_pti与线段pti_pmi的比值：/>(或/>),线段pni_oi与线段oi_pti的比值：/>(或/>)。(在此获取这些线段比例的目的是，在摄像机图像上按比例求取oi对应关键的二维像素坐标；选取3个参考点时，要尽量使γ和β值大于0且取较大值，也要尽量使直线pki_pti与直线pti_pmi的夹角的正弦值取较大值，当γ和β值大于0时，s取较大值，也有利于最终结果的精度，在无法同时满足以上情况时，要根据实际情况做好平衡)。其中，摄像机相关的坐标系有三种，分别为，摄像机坐标系，图像物理坐标系，图像像素坐标系，图像像素坐标系的原点为图像左上角，摄像机坐标系X轴Y轴的方向分别与图像物理坐标系的x轴y轴方向、图像像素坐标系的u轴v轴方向相同(由于透镜效应，图像未加处理时是左右上下颠倒的，但一般情况下摄像机自带软件会自动flip的，如果没有要自行矫正过来)。值得一提的是，高精度物体姿态估计阶段的具体步骤包括步骤SW3.3：藉由深度神经网络识别出来的，在摄像头获取的图像中参考点对应的非共线的三个关键点的位置，记为(uk,vk)、(um,vm)、(un,vn)，所述三个关键点对应的参考点在参考平面上的垂直投影点为pki,pmi,pni，由上述的先验知识信息，上述垂直投影点相对于局部坐标系axis_i的坐标为已知。那么，投影点pki,pmi,pni在摄像头图像中的位置可参考示意图(附图的图14)求得，方法如下面的①所示，结果可简记为(uki,vki),(umi,vmi),(uni,vni)，那么，pti在摄像头获取的图像中的像素坐标值为((uki+γ*umi)/(γ+1),(vki+γ*vmi)/(γ+1))，要通过选用等效的表达方式确保γ不等于-1且γ的分母不为零，简记为(uti，vti)，继而原点oi在摄像头获取的图像中的像素坐标值为((uni+β*uti)/(β+1),(vni+β*vti)/(β+1)),要通过选用等效的表达方式确保β不等于-1且β的分母不为零，简记为(uoi，voi)。同样的方法，即可解出点(s,0,0),(0,s,0)在摄像机图像上的二维像素坐标值。对于(0,0,s)点，借助已解得的oi在摄像头图像中的像素坐标，可按如下②的方法得出其坐标。继而，此四点在摄像机坐标系上的三维坐标值可按如下③的方法得出。已知摄像机内参矩阵：/>f为焦距，dx、dy为感光芯片单个感光元在u方向和v反向尺寸，一般可以在摄像机手册中查到或通过标定获得。

值得一提的是，高精度物体姿态估计阶段的具体步骤包括步骤SW3.4：藉由上述的先验知识信息以及关键点和垂直投影点的坐标信息，获得axis_i坐标系上的(0,0,0)，(s,0,0)，(0,s,0)，(0,0,s)点对应摄像机图像像素坐标系的像素坐标，以及摄像机坐标系上的三维坐标值，继而采用ransac，ict等方法，或直接带入变换方程求解方程组求取axis_i坐标系到摄像头坐标系的变换关系(Rci,Tci)(由于上述四点的图像像素坐标是在充分利用参考平面的情况下获得，所以也可以再一次运用pnp相关方法求取axis_i坐标系到摄像头坐标系的变换关系)，这样，就能非常充分的利用平面的参考作用，可保证最终结果的高精度、高稳定性，进一步结合物体坐标系到axis_i坐标系的转换关系(Ri,Ti)，就可以获得物体相对于摄像头坐标系的高精度实时姿态(R,T)。

①其中，以求pm1(i＝1时，pmi为pm1)在摄像机像素坐标系上对应关键点的位置为例(参见附图的图14，注：单位要统一，如长度单位为米，角度单位为弧度)：

已知摄像机坐标系到图像像素坐标系的转换公式：(旁标c代表属摄像机坐标系)，简记为formula_1。i.当0<θ<90°时：

o_e＝f(焦距)，e_g＝(v0-vm)*dy*cosα+(u0-um)*dx*sinα(参见图8，虚线轴系为现有轴系，按前面的约定，为确保上下文一致性，此时旋转角度为-α)；如附图的图14，/> 对应摄像机从虚线坐标系转换到实线坐标系，um_、vm_为转换后的图像像素坐标，/> (其中运用了摄像机坐标系到图像像素坐标系的转换公式formula_1)，再次运用公式formula_1，可得

其中，um1_，vm1_为点pm1在摄像机实线坐标系对应的图像像素坐标系上的坐标，将上面的um1_、vm1_带入下面两个式子即可得到um1、vm1，um1、vm1为点pm1在摄像机虚线坐标系(现有轴系)对应的图像像素坐标系上的坐标，为使表达简便，未深带入，um1＝vm1_*sinα+um1_*cosα，vm1＝vm1_*cosα-um1_*sinα。

ii.当θ＝90°时：

同理，可求得i＝1或2时的(uki,vki),(umi,vmi),(uni,vni)。

②i.当0<θ<90°时(注：单位要统一，如长度单位为米，角度单位为弧度):

可参见附图的图14，i＝1时，s按上面约定取0.1，按上述方法解得axsi_1坐标原点o1在摄像头图像中的像素坐标(uo1,vo1)，对于(0,0,s)点，求取其摄像头图像像素坐标的过程如下：

(0,0,s)点到摄像机轴系xy平面的距离/>对应摄像机从虚线坐标系转换到实线坐标系，uo1_、vo1_为转换后的图像像素坐标，/>

其中，u00s_、v00s_为(0,0,s)点在摄像机实线坐标系对应的图像像素坐标系上的坐标，将上面的u00s_、v00s_带入下面两个式子即可得到u00s、v00s，u00s、v00s为(0,0,s)点在摄像机虚线坐标系(现有轴系)对应的图像像素坐标系上的坐标，为使表达简便，未深带入，u00s＝v00s_*sinα+u00s_*cosα，v00s＝v00s_*cosα-u00s_*sinα。

ii.当θ＝90°时：

i＝1时，s按上面约定取0.1，借助按上述方法解得的o1在摄像头图像中的坐标(uo1,vo1)，对于(0,0,s)点，求取其在摄像头图像中的像素坐标的过程如下：

同理，可求得i＝2时(0,0,s)点在摄像机像素坐标系上对应关键点的坐标。

③i.当0<θ<90°时(注：单位要统一，如长度单位为米，角度单位为弧度)：

可参见附图的图14，i＝1时，以o1点为例((uo1,vo1)为o1点对应的图像像素坐标系上的坐标，可按上述方法求得)： 那么即是oi点在摄像机坐标系中的z值，等同记为zc_o1，再根据摄像机坐标系到图像像素坐标系的转换公式formula_1，可求得oi点在摄像机坐标系的x、y值,即

ii.当θ＝90°时：

同理，可求得i＝1或2时上述4点(即(0,0,0)、(s,0,0)、(0,s,0)、(0,0,s)点)在摄像机坐标系上的三维坐标。要确保上述①②③中相关式子的分母不为0。

值得一提的是，附图的图15、图16、图17是θ＝90°时，一种最终效果示意图：深色轴系为标准姿态，浅色轴系为预测的姿态，连续三帧图像，可以看到，具有较高的精度和稳定性。

第一变形实施例。

第一变形实施例作为以上任一个实施例(例如，优选实施例、第一实施例等)的变形实施例，与对应的各个实施例，基本相同，区别在于，不再部分地或者完整地参考附图的图1至图17。

值得一提的是，本发明各个实施例还公开了一种基于参考平面的高精度物体多自由度姿态估计系统，用于实施以上任一个实施例的任一种技术方案公开的基于参考平面的高精度物体多自由度姿态估计方法的步骤。

值得一提的是，本发明各个实施例还公开了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如以上任一个实施例的任一种技术方案公开的所述基于参考平面的高精度物体多自由度姿态估计方法的步骤。

值得一提的是，本发明各个实施例还公开了一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现如以上任一个实施例的任一种技术方案公开的所述基于参考平面的高精度物体多自由度姿态估计方法的步骤。

值得一提的是，本发明专利申请涉及的摄像头的具体选型等技术特征应被视为现有技术，这些技术特征的具体结构、工作原理以及可能涉及到的控制方式、空间布置方式采用本领域的常规选择即可，不应被视为本发明专利的发明点所在，本发明专利不做进一步具体展开详述。

对于本领域的技术人员而言，依然可以对前述各实施例所记载的技术方案进行修改，或对其中部分技术特征进行等同替换，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围。

Claims (8)

1.一种基于参考平面的高精度物体多自由度姿态估计方法，其特征在于，用于多自由度地估计一物体，上述物体相对于预设的一参考平面设置，包括以下步骤：

步骤S1：由至少一成像设备实时成像并且实时输出包含待多自由度姿态估计的上述物体的一信息流，上述物体相对于上述参考平面具有有限个稳定的多自由度状态；

步骤S1具体实施为以下步骤：由至少一摄像头实时成像并且实时输出包括待多自由度姿态估计的上述物体的至少一图片；

步骤S2：至少一存储设备被事先预置或者被实时录入上述物体和/或上述成像设备的至少一先验知识信息；

步骤S2中，所述先验知识信息藉由测量、标定，或向设备厂家索取，或查相应手册方式获得，所述先验知识信息包括但不限于：

摄像头内参数；

待多自由度姿态估计的上述物体的三维数模；

物体在状态i下参考点到参考平面的距离，记为di1,di2,di3,…，i代表第i种稳定状态,参考点可在物体上自由选取；

测量摄像头光轴线与参考平面的夹角，记为θ；

当0<θ<90°时,测量摄像头绕其光轴线旋转的角度α，以摄像头X轴与参考平面平行Y轴正方向在靠近上述参考平面一侧时α＝0，沿着摄像头的Z轴，右手法则，逆时针方向α为正，顺时针方向α为负，当θ＝90°时，不需要测量α值；

测量摄像头坐标系原点到参考平面的距离，记为hc；

步骤S3：根据所述先验知识信息，结合预设的一关键点识别方法，实时获取并且输出上述物体相对于一摄像机坐标系的多自由度姿态。

2.根据权利要求1所述的基于参考平面的高精度物体多自由度姿态估计方法，其特征在于，步骤S3具体实施为以下任一条步骤：实时生成上述物体相对于所述摄像头的一摄像头坐标系的一姿态，或者实时生成上述物体的至少一点相对于所述摄像头的一摄像头坐标系的至少一坐标。

3.根据权利要求2所述的基于参考平面的高精度物体多自由度姿态估计方法，其特征在于，步骤S3中，所述关键点识别方法，具体实施为以下任一类步骤的集合：

步骤SP3.1：首先对物体进行目标检测，输入的是图像，输出的是目标位置，常用包含目标物的方框表示，用输出的方框crop图像，如果是多个目标，用padding统一尺寸后堆叠在一起形成一个batch送入关键点识别网络；

步骤SP3.2：用diamondnet实现物体关键点识别；

步骤SP3.3：用各种pnp方法粗略估计物体姿态，目的是确定物体相对于参考平面处于哪种状态；

步骤SP3.4：确认当前状态i；或者：

步骤S3中，所述关键点识别方法，具体实施为以下步骤：

步骤SQ3.1：如果待检测的物体种类为一种或几种，且待识别的关键点数较少，藉由diamondnet加上关键点heatmap局部非极大值抑制以及关键点聚类的方法，同时实现关键点识别与物体检测；

步骤SQ3.2：用各种pnp方法粗略估计物体姿态，目的是确定物体相对于参考平面处于哪种状态；

步骤SQ3.3：确认当前状态i；或者：

藉由现有的物体6d姿态估计方法确认物体处于哪种稳定状态i；用关键点识别网络识别出图像上物体的关键点。

4.根据权利要求3所述的基于参考平面的高精度物体多自由度姿态估计方法，其特征在于，所述关键点识别方法，还包括以下步骤：

步骤SW3.1：藉由两阶段信息，计算出坐标轴axis_i上的典型点(0,0,0)，(s,0,0)，(0,s,0)，(0,0,s)，其中，s可取大于0的任意值，在摄像机图像像素坐标系的二维坐标值及摄像机坐标系的三维坐标值；

步骤SW3.2：先求取axis_i坐标原点也就是axis_i坐标下(0,0,0)点在摄像机图像像素坐标系的二维坐标值。

5.根据权利要求4所述的基于参考平面的高精度物体多自由度姿态估计方法，其特征在于，所述关键点识别方法，还包括以下步骤：

步骤SW3.3：藉由深度神经网络识别出来的，在摄像头获取的图像中参考点对应的非共线的三个关键点的位置，记为(uk,vk)、(um,vm)、(un,vn)，所述三个关键点对应的参考点在参考平面上的垂直投影点记为pki,pmi,pni，由上述的先验知识信息，上述垂直投影点相对于局部坐标系axis_i的坐标为已知；

步骤SW3.4：藉由上述的先验知识信息以及关键点和垂直投影点的坐标信息，获得axis_i坐标系上的(0,0,0)，(s,0,0)，(0,s,0)，(0,0,s)点对应摄像机图像像素坐标系的像素坐标，并求得axis_i坐标系上的(0,0,0)，(s,0,0)，(0,s,0)，(0,0,s)点在摄像机坐标系的坐标，继而采用ransac，ict或直接带入变换方程求解方程组的方法求取axis_i坐标系到摄像机坐标系的变换关系，由于上述axis_i坐标系上的(0,0,0)，(s,0,0)，(0,s,0)，(0,0,s)点对应的摄像机图像像素坐标是基于参考平面获得，所以选择性地再次运用各种pnp方法求取axis_i坐标系到摄像机坐标系的变换关系，再进一步结合物体坐标系到axis_i坐标系的转换关系，获得物体相对于摄像头坐标系的高精度实时姿态。

6.一种基于参考平面的高精度物体多自由度姿态估计系统，其特征在于，用于实施如权利要求1-5中任一项权利要求所述的基于参考平面的高精度物体多自由度姿态估计方法的步骤。

7.一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-5中任一项权利要求所述的基于参考平面的高精度物体多自由度姿态估计方法的步骤。

8.一种非暂态计算机可读存储介质，其上存储有计算机程序，其特征在于，该计算机程序被处理器执行时实现如权利要求1-5中任一项权利要求所述的基于参考平面的高精度物体多自由度姿态估计方法的步骤。