一种基于PNP的3D定位跟踪方法和系统技术方案

本发明专利技术公开了一种基于PNP的3D定位跟踪方法和系统，所述方法包括：离线状态下，先进行相机标定获取PNP定位模型所需的相机内参，然后基于PNP定位方法示教一个靶标坐标系与相机坐标系的一个标准位置关系，再进行手眼标定确定相机坐标系与机械手工具坐标系之间的转换关系；靶标做无规则运动时，相机实时对其拍照获取靶标图像；基于PNP方法进行靶标3D定位，计算此拍照时刻靶标坐标系在相机坐标系中的位姿；计算保持相机与靶标示教位置不变时，机械手工具坐标系在基坐标系中的位姿，即机械手需要移动到的位置；根据位姿控制机械手移动。本发明专利技术采用PNP方法进行3D定位，可实现高精度定位，需要用于定位的数据较少，定位速度较快。定位速度较快。定位速度较快。

【技术实现步骤摘要】
一种基于PNP的3D定位跟踪方法和系统


[0001]本专利技术涉及目标追踪
，具体涉及一种基于PNP的3D定位跟踪方法和系统。

技术介绍

[0002]随着工业自动化技术的发展，机械手已被广泛应用于工业的多种领域，主要通过多个自由度的机械手完成一些简单的装配制造工作，工业机械手大多用于流水线作业，其工作过程是基于既定的程序完成单调重复的操作，较为缺乏自主性与决策性。现有技术中，有采用3D成像的方式进行数据采集，以实现对目标物的定位，再结合对机械手的控制，从而使机械手能够移动至目标物位置并进行相应操作。采用3D成像的方式进行数据采集，此方式数据较丰富，但相对而言，采集的速度较慢。而目标跟踪要求速度较快，定位也不需要太多数据，且相较而言，一般的3D成像的精度也不高，故传统的3D成像方式不合适快速地3D定位跟踪的要求。

技术实现思路

[0003]针对上述技术问题，本专利技术提供了一种定位速度快，精度高的基于PNP的3D定位跟踪方法和系统。
[0004]本专利技术实施例提供一种基于PNP的3D定位跟踪方法，所述方法包括：离线状态下，先进行相机标定，获取PNP定位模型所需的相机内参，然后基于PNP定位方法示教一个靶标坐标系与相机坐标系的一个标准位置关系，再进行手眼标定确定相机坐标系与机械手工具坐标系之间的转换关系；靶标做无规则运动时，相机实时对其拍照获取靶标图像；基于PNP方法进行靶标3D定位，计算此拍照时刻，靶标坐标系在相机坐标系中的位姿；计算保持相机与靶标示教位置不变时，机械手工具坐标系在机械手基坐标系中的位姿，即机械手需要移动到的位置；根据位姿控制机械手移动。
[0005]可选地，所述计算保持相机与靶标示教位置不变时，机械手工具坐标系在机械手基坐标系中的位姿，即机械手需要移动到的位置的步骤包括：通过PNP方法计算靶标坐标系在相机坐标系中的位姿；通过相机坐标系与机械手工具坐标系的位置关系，机械手工具坐标系在机械手基坐标系中的位姿，计算实时的靶标坐标系在机械手基坐标系中的位姿；相机坐标系与机械手工具坐标系位置保持不变时，计算机械手工具坐标系在机械手基坐标系的位姿。
[0006]可选地，所述相机标定为单相机标定，所述单相机标定相机内参，建立单目相机PNP定位模型，空间任一点(Xw，Yw，Zw)与像素坐标(u,v)的转换关系为
[0007][0008]其中，f为焦距、r为畸变因子、S
x
、S
y
为像元尺寸，u0为像素偏移、v0为相机内参。
[0009]优选地，所述靶标坐标系，相机坐标系，机械手工具坐标系和机械手基坐标系中，任两个坐标系之间的转换关系为
[0010][0011]其中，R3
×
3为旋转矩阵，T3
×
1为平移矩阵。
[0012]优选地，所述相机固定于机械手上，跟随机械手移动。
[0013]优选地，所述机械手采用轴动模式运动。
[0014]优选地，所述相机为高帧率相机。
[0015]优选地，所述相机内参包括相机的焦距。
[0016]本专利技术的另一实施例还提供一种基于PNP的3D定位跟踪系统，所述系统包括控制装置和六自由度机械手，六自由度机械手上设置有相机，所述控制装置用于执行上述任一项所述的基于PNP的3D定位跟踪方法。
[0017]本专利技术实施例提供的技术方案中，离线状态下，先进行相机标定，获取PNP定位模型所需的相机内参，然后基于PNP定位方法示教一个靶标坐标系与相机坐标系的一个标准位置关系，再通过手眼标定确定相机坐标系与机械手工具坐标系之间的转换关系；靶标做无规则运动时，相机实时对其拍照获取靶标图像；基于PNP方法进行靶标3D定位，计算此拍照时刻，靶标坐标系在相机坐标系中的位姿；计算保持相机与靶标示教位置不变时，机械手工具坐标系在机械手基坐标系中的位姿，即机械手需要移动到的位置；相比于现有技术，本专利技术采用PNP方法进行3D定位，标靶定制，定制地标靶可以做的非常高的精度，可以实现高精度定位，需要用于定位的数据较少，定位速度较快。
附图说明
[0018]图1为本专利技术一种基于PNP的3D定位跟踪方法的流程示意图；
[0019]图2为本专利技术四个坐标系的示意图；
[0020]图3为本专利技术基于PNP的3D定位跟踪方法的另一实施例的流程示意图；
[0021]图4为本专利技术针孔成像透视投影模型；
[0022]图5为本专利技术PNP定位的原理图；
[0023]图6为本专利技术手眼标定的原理图。
具体实施方式
[0024]下面将结合本专利技术实施例中的附图，对本专利技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本专利技术一部分实施例，而不是全部的实施例。基于本专利技术中的实施例，本领域技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本专利技术保护的范围。
[0025]本专利技术提供一种运动状态下目标追踪系统，所述系统包括旋转台和控制装置，旋转台上设置有六自由度机械手，通过旋转台带动六自由度机械手转动，六自由度机械手上设置有相机，通过六自由度机械手控制相机移动，从而通过相机进行3D跟踪，所述控制装置采用如下实施例所述的基于PNP的3D定位跟踪方法。
[0026]请参阅图1所示，本专利技术提供一种基于PNP的3D定位跟踪方法，所述方法包括如下步骤：
[0027]步骤S10，离线状态下先进行相机标定，获取PNP定位模型所需的相机内参，然后基于PNP定位方法示教一个靶标坐标系与相机坐标系的一个标准位置关系，再进行手眼标定确定相机坐标系与机械手工具坐标系之间的转换关系。
[0028]目标追踪设备的模型中，包括四个坐标系，请参考图2所示，分别为机械手基坐标系，机械手工具坐标系，相机坐标系和靶标坐标系，目标追踪的过程即靶标在做无规则运动时，相机会保持对其进行拍照，定位，然后计算出保持相机与靶标的相对位置不变时，机械手需要走到的位姿。
[0029]在本实施例中，通过对单相机进行标定，可以获取PNP定位模型所需的相机内参，然后示教一个靶标坐标系在相机坐标系中的位姿，也就是靶标坐标系与相机坐标系之间的转换关系，具体通过获得所述单目相机的内部参数和外部参数，根据相机的内参以及特制靶标上的坐标系，基于PNP方法计算靶标坐标系在相机坐标系中的位姿，即为后续实时追踪所需的示教位置，其中，所述内部参数包括所述单目相机成像过程中沿像素排列水平方向的像素焦距参数和光轴偏移量参数、沿像素排列水平方向的像素焦距参数和光轴偏移量参数沿像素排列竖直方向的像素焦距参数和光轴偏移量参数、以及径向畸变参数和切向畸变参数。所述外部参数包括旋转参数和平移参数。
[0030]机械手工作坐标系在机械手基坐标系中的位姿需在每次拍照时访问机械手获取，具体的，这里的拍照是指的是每次拍图时的这个位置以及这个时刻，因为相机在机械手上，拍照的位置实时都在变动，工具坐标系在机械手坐标系中的位置主要是通过机械手程序告知软件，或者软件主动去访问机械手本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于PNP的3D定位跟踪方法，其特征在于，所述方法包括：离线状态下，先进行相机标定，获取PNP定位模型所需的相机内参，然后基于PNP定位方法示教一个靶标坐标系与相机坐标系的一个标准位置关系，再进行手眼标定确定相机坐标系与机械手工具坐标系之间的转换关系；靶标做无规则运动时，相机实时对其拍照获取靶标图像；基于PNP方法进行靶标3D定位，计算此拍照时刻，靶标坐标系在相机坐标系中的位姿；计算保持相机与靶标示教位置不变时，机械手工具坐标系在机械手基坐标系中的位姿，即机械手需要移动到的位置；根据位姿控制机械手移动。2.根据权利要求1所述的基于PNP的3D定位跟踪方法，其特征在于，所述计算保持相机与靶标示教位置不变时，机械手工具坐标系在机械手基坐标系中的位姿，即机械手需要移动到的位置的步骤包括：通过PNP方法计算靶标坐标系在相机坐标系中的位姿；通过相机坐标系与机械手工具坐标系的位置关系，机械手工具坐标系在机械手基坐标系中的位姿，计算实时的靶标坐标系在机械手基坐标系中的位姿；相机坐标系与机械手工具坐标系位置保持不变时，计算机械手工具坐标系在机械手基坐标系的位姿。3.根据权利要求1所述的基于PNP的3D定位跟踪方法，其特征在于，所述相机标定为单相机标定，所述单相机标定相机内参，建立单目相机PNP...

【专利技术属性】
技术研发人员：侯梦华，陈凯，梅文宝，刘俊锋，彭家豪，
申请(专利权)人：深圳市启灵图像科技有限公司，
类型：发明
国别省市：