用于机械臂遥操作的多摄像机联合主动跟踪目标的方法技术

本发明专利技术涉及一种用于机械臂遥操作的多摄像机联合主动跟踪目标的方法，属于机械臂遥操作领域。本发明专利技术在遥操作基础上，将多个摄像机安放在不同角度，对摄像机进行标定，采用粒子滤波和sift局部特征匹配进而实现联合主动跟踪，保证待跟踪目标始终处于视场中央，对机械臂各个角度进行监控，并避免因遮挡等因素引起的跟踪失败，并加入人机交互，提高跟踪的鲁棒性。本发明专利技术方法在遥操作过程中，通过人机交互方式标注新的目标，使目标始终处在操作员的观察区域，或者更新目标，方便操作员制定下一步的控制策略。

【技术实现步骤摘要】

【技术保护点】
一种用于机械臂遥操作的多摄像机联合主动跟踪目标的方法，其特征在于该方法包括以下步骤：(1)将多个摄像机置于机械臂的左上、右上和正面，对摄像机进行标定，分别获得多台摄像机的内参数矩阵M1、外参数矩阵M2、畸变系数和位姿变换矩阵T12,T23,T34……，具体步骤如下：(1‑1)设空间点P在摄像机坐标系中的坐标为P(xC,yC,zC)，使空间点P沿着通过光心的光线投影到一个图像平面上，在图像平面中建立图像坐标系，得到该空间点P在图像坐标系中的投影坐标为P(x,y)，将该投影坐标表示为计算机图像坐标系中的坐标为P(u,v)，由小孔成像原理得到，空间点P在摄像机坐标中的坐标(xC,yC,zC)与投影坐标P(x,y)的坐标变换关系为：x=fxCzCy=fyCzC,]]>其中f为摄像机焦距；(1‑2)设定一个成像单元，该成像单元在上述图像坐标系中的x轴和y轴方向上的物理尺寸分别为dx和dy，成像单元中任意一个像素在上述计算机图像坐标系下的坐标(u,v)和上述图像坐标系下的坐标(x,y)存在以下坐标变换关系：u=xdx+u0v=ydy+v0,]]>其中，O(u0,v0)为位于摄像机主光轴上任意一点在计算机图像坐标系中的成像坐标，该成像坐标即主点坐标；(1‑3)根据上述步骤(1‑1)和步骤(1‑2)的坐标变换关系，得到摄像机的内参数矩阵M1为：M1=fdx0u00fdyv0001,]]>其中，为摄像机焦距f在摄像机图像平面坐标系的u轴上的归一化焦距，为摄像机焦距f在摄像机图像平面坐标系的v轴上的归一化焦距；(1‑4)建立世界坐标系，设空间点P在世界坐标系中的坐标为(xW,yW,zW)，空间点P在摄像机坐标系中的坐标和在世界坐标系中的坐标存在以下关系：xCyCzC=RtxWyWzW1,]]>其中，R为3×3的单位正交矩阵，t为摄像机坐标系和世界坐标系之间的三维平移向量，定义其中的[R t]为摄像机外参数矩阵M2；(1‑5)根据上述步骤(1‑3)和步骤(1‑4)得到的摄像机内参数M1和外参数M2，得到空间点P在世界坐标系中的坐标与空间点P在计算机图像坐标系中的投影坐标之间的投影变换关系为：P＝M1M2；(1‑6)设摄像机的径向畸变参数为k1、k2和k3，使k1、k2和k3满足以下方程组：x=xu(1+k1r2+k2r4+k3r6)y=yu(1+k1r2+k2r4+k3r6),]]>求解该方程组，得到摄像机的径向畸变参数k1、k2和k3，其中，(x,y)为空间点P在图像坐标系中的原始位置坐标，(xu,yu)为空间点P通过成像原理得到的理想位置坐标；设摄像机的切向畸变参数为p1和p2，使p1和p2满足以下方程组：x=xu+2p1xuyu+p2(r2+2xu2)y=yu+2p2xuyu+p1(r2+2yu2),]]>求解该方程组，得到摄像机的切向畸变参数p1和p2；(1‑7)根据上述步骤(1‑6)得到的径向畸变参数为k1、k2和k3和切向畸变参数p1和p2，空间点P的原始位置坐标与理想位置坐标之间存在以下转换关系：x=xu(1+k1r2+k2r4+k3r6)+2p1xuyu+p2(r2+2xu2)y=yu(1+k1r2+k2r4+k3r6)+2p2xuyu+p1(r2+2yu2);]]>空间点P的理想位置坐标与原始位置坐标之间存在以下转换关系：xu=x-2p1xy+p2(r2+2x2)1+k1r2+k2r4+k3r6yu=y-2p2x y+p1(r2+2x2)1+k1r2+k2r4+k3r6;]]>(1‑8)遍历多个摄像机中的每个摄像机，重复步骤(1‑1)‑步骤(1‑7)，完成摄像机的标定；(1‑9)设多个摄像机中的第一个摄像机和第二个摄像机之间的旋转矩阵为R12，第一个摄像机和第二个摄像机之间的平移矩阵为Tran12，得到第一个摄像机和第二个摄像机之间的位姿变换矩阵T12：T12＝[R12 Tran12]；(1‑10)遍历多个摄像机中的每任意两个摄像机，重复步骤(1‑9)，得到多个摄像机的位姿变换矩阵T12,T23,T34……；(2)使经过上述步骤(1)标定后的多台摄像机主动跟踪待抓取目标，具体包括以下步骤：(2‑1)将多个经过上述步骤(1)标定后的摄像机置于机械臂的左上、右上和正面，设待抓取目标中心S在摄像机成像平面Pd上的投影为S'，计算S'与摄像机成像平面的中心C之间的距离d，设定距离阈值th，对距离d进行判断，若d＞th，则向摄像机发送一个调整指令，使摄像机镜头进行使距离d减小的方向转动，直到d≤th，若d≤th，则使摄像机镜头保持原来位置；(2‑2)采用粒子滤波跟踪算法，摄像机联合主动跟踪待抓取目标，具体步骤如下：(2‑2‑1)使多台摄像机中的任意一台摄像机获取待抓取目标的视频图像序列，对视频图像序列中t0时刻的图像进行手工标注，标注出其中...

【技术特征摘要】

【专利技术属性】
技术研发人员：孙富春，刘华平，滕辉，
申请(专利权)人：清华大学，
类型：发明
国别省市：北京;11