说明书用于机械臂遥操作的多摄像机联合主动跟踪目标的方法
技术领域
本发明涉及一种用于机械臂遥操作的多摄像机联合主动跟踪目标的方法，属于机械臂遥操作领域。
背景技术
对于机械臂的遥操作，视觉系统是机械臂遥操作中的一个关键技术，其为操作臂提供目标物体的实时图像信息和空间位姿信息。同时视觉系统还将操作臂所处的状态、环境、以及工作阶段直观的反馈给地面操作员。
目前的遥操作方法中，摄像机提供了图像信息，并没有利用图像信息实现对机械臂工作过程中特定目标的主动跟踪，且摄像头都保持不变，不能主动调整视角，没有考虑机械臂运动超过摄像头视场而无法监控的情况。
现有的技术文献中，发明专利“基于Kinect的空间遥操作机器人的控制系统及其方法”，公开号为CN201310193564.7，利用Kinect实现三维环境建模，并对预测环境进行一致性校正。该方法的缺点是，只采用一台摄像机对机械臂区域进行拍摄，并且没有对目标实现跟踪。另外，该方法摄像头保持不动，无法确保机械臂始终在视场中。
发明内容
本发明的目的是提出一种用于机械臂遥操作的多摄像机联合主动跟踪目标的方法，在遥操作基础上，为机械臂遥操作设计一套多个摄像头主动视觉系统，以实时采集、传输图像，为操作员提供直观的视觉观测依据。
本发明提出的用于机械臂遥操作的多摄像机联合主动跟踪目标的方法，包括以下步骤：
(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-2p2xy+p1(r2+2x2)1+k1r2+k2r4+k3r6;]]>
(1-8)遍历多个摄像机中的每个摄像机，重复步骤(1-1)-步骤(1-7)，完成摄像机的标定；
(1-9)设多个摄像机中的第一个摄像机和第二个摄像机之间的旋转矩阵为R12，第一个摄像机和第二个摄像机之间的平移矩阵为Tran12，得到第一个摄像机和第二个摄像机之间的位姿变换矩阵T12：T12＝[R12Tran12]；
(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时刻的图像进行手工标注，标注出其中的待抓取目标区域；
(2-2-2)以上述待抓取目标区域的中心为原点，产生一个粒子集{}，其中m为粒子集中的粒子个数，m＝1,...,M，每个粒子代表一个待抓取目标可能存在的区域；设粒子集{}中的粒子的运动服从正态分布，粒子集{}中的每个粒子独立传播，得到t时刻的粒子集{}和t+1时刻的粒子集{}；
(2-2-3)设t0时刻粒子集{}的参考直方图为参考直方图q*共有L个灰度级，设t时刻粒子集{}的颜色直方图为qt(x)＝qt(n；x),n＝1,2...L，x为粒子集{}中的粒子，对t时刻粒子集{}中的每个粒子独立传播后，得到的t+1时刻粒子集{}进行观测，得到粒子集{}中每个粒子区域的颜色直方图和参考直方图，计算粒子集}中每个粒子区域的颜色直方图和参考直方图之间的巴氏距离D：定义粒子权值为ω，使ω＝D，N的取值为200；
(2-2-4)对上述粒子权值进行后验概率计算，得到t+1时刻粒子的存在概率期望值E(xt+1)：其中，ωt+1是t+1时刻的每个粒子权值；
(2-2-5)将上述期望值E(xt+1)作为待抓取目标在t+1时刻的存在概率最优估计，并将上述粒子集{}中的存在概率最优粒子所覆盖的区域的中心作为待抓取目标区域中心；
(2-2-6)重复上述步骤(2-2-2)～步骤(2-2-5)，得到待抓取目标在时刻t到下一时刻t+1的存在概率最优估计和待抓取目标区域中心；
(2-2-7)重复步骤(2-1)，使摄像机镜头对准上述步骤(2-2-6)的待抓取目标区域中心；
(2-2-8)提取上述步骤(2-2-7)的待抓取目标区域的局部特征点；
(2-2-9)使多台摄像机中的其他几台摄像机获取待抓取目标的视频图像序列，从多个视频图像序列中分别提取局部特征点；
(2-2-10)将步骤(2-2-8)提取到的待抓取目标区域的局部特征点与步骤(2-2-9)提取到的所有局部特征点进行匹配，得到多台摄像机中的其他几台摄像机的待抓取目标区域的精确区域；
(2-2-11)重复上述步骤(2-2-2)～步骤(2-2-10)，使每台摄像机分别跟踪得到的待抓取目标区域的精确区域，实现多摄像机联合主动跟踪目标。
本发明提出的用于机械臂遥操作的多摄像机联合主动跟踪目标的方法，具有以下优点：
1、本发明的用于机械臂遥操作的多摄像机联合主动跟踪方法，采用多个摄像机，实现对特抓取目标的主动跟踪。
2、本发明中的多个摄像机，安放在不同角度，进行联合主动跟踪，被跟踪目标始终处于镜头中心，对机械臂各个角度进行监控，并避免因遮挡、背景混乱等因素引起的跟踪失败。
3、本发明采用了粒子滤波跟踪算法和sift特征点匹配，实现多个摄像机联合跟踪，提高了摄像机跟踪的鲁棒性。
附图说明
图1是本发明方法中摄像机成像原理示意图。
图2是本发明方法中摄像机控制模型示意图
具体实施方式
本发明提出的用于机械臂遥操作的多摄像机联合主动跟踪目标的方法，包括以下步骤：
(1)将多个摄像机置于机械臂的左上、右上和正面，对摄像机进行标定，分别获得多台摄像机的内参数矩阵M1、外参数矩阵M2、畸变系数和位姿变换矩阵T12,T23,T34……，具体 步骤如下：
(1-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-2p2xy+p1(r2+2x2)1+k1r2+k2r4+k3r6;]]>
(1-8)遍历多个摄像机中的每个摄像机，重复步骤(1-1)-步骤(1-7)，完成摄像机的标定；
(1-9)设多个摄像机中的第一个摄像机和第二个摄像机之间的旋转矩阵为R12，第一个摄像机和第二个摄像机之间的平移矩阵为Tran12，得到第一个摄像机和第二个摄像机之间的位姿变换矩阵T12：T12＝[R12Tran12]；
(1-10)遍历多个摄像机中的每任意两个摄像机，重复步骤(1-9)，得到多个摄像机的位姿变换矩阵T12,T23,T34……；
(2)使经过上述步骤(1)标定后的多台摄像机主动跟踪待抓取目标，具体包括以下步骤：
(2-1)将多个经过上述步骤(1)标定后的摄像机置于机械臂的左上、右上和正面，设待抓取目标中心S在摄像机成像平面Pd上的投影为S'，如图2所示，计算S'与摄像机成像平面的中心C之间的距离d，设定距离阈值th，对距离d进行判断，若d＞th，则向摄像机发送一个调整指令，使摄像机镜头进行使距离d减小的方向转动，直到d≤th，若d≤th，则使摄像机镜头保持原来位置；
(2-2)采用粒子滤波跟踪算法，摄像机联合主动跟踪待抓取目标，具体步骤如下：
(2-2-1)使多台摄像机中的任意一台摄像机获取待抓取目标的视频图像序列，对视频图像序列中t0时刻的图像进行手工标注，标注出其中的待抓取目标区域；
(2-2-2)以上述待抓取目标区域的中心为原点，产生一个粒子集{}，其中m为粒子集中的粒子个数，m＝1,...,M，每个粒子代表一个待抓取目标可能存在的区域；设粒子集{}中的粒子的运动服从正态分布，粒子集{}中的每个粒子独立传播，得到t时刻的粒子集{}和t+1时刻的粒子集{}；
(2-2-3)设t0时刻粒子集{}的参考直方图为参考直方图q*共有L个灰度级，设t时刻粒子集{}的颜色直方图为qt(x)＝qt(n；x),n＝1,2...L，x为粒子集{}中的粒子，对t时刻粒子集{}中的每个粒子独立传播后，得到的t+1时刻粒子集{}进行观测，得到粒子集{}中每个粒子区域的颜色直方图和参考直方图，计算粒子集{}中每个粒子区域的颜色直方图和参考直方图之间的巴氏距离D：定义粒子权值为ω，使ω＝D，N的取值为200；
(2-2-4)对上述粒子权值进行后验概率计算，得到t+1时刻粒子的存在概率期望值E(xt+1)：其中，ωt+1是t+1时刻的每个粒子权值；
(2-2-5)将上述期望值E(xt+1)作为待抓取目标在t+1时刻的存在概率最优估计，并将上述粒子集{}中的存在概率最优粒子所覆盖的区域的中心作为待抓取目标区域中心；
(2-2-6)重复上述步骤(2-2-2)～步骤(2-2-5)，得到待抓取目标在时刻t到下一 时刻t+1的存在概率最优估计和待抓取目标区域中心；
(2-2-7)重复步骤(2-1)，使摄像机镜头对准上述步骤(2-2-6)的待抓取目标区域中心；
(2-2-8)提取上述步骤(2-2-7)的待抓取目标区域的局部特征点；
(2-2-9)使多台摄像机中的其他几台摄像机获取待抓取目标的视频图像序列，从多个视频图像序列中分别提取局部特征点(即sift局部特征点)，
(2-2-10)将步骤(2-2-8)提取到的待抓取目标区域的局部特征点与步骤(2-2-9)提取到的所有局部特征点进行匹配，得到多台摄像机中的其他几台摄像机的待抓取目标区域的精确区域；
(2-2-11)重复上述步骤(2-2-2)～步骤(2-2-10)，使每台摄像机分别跟踪得到的待抓取目标区域的精确区域，实现多摄像机联合主动跟踪目标。