和第六伺服电机。第一伺服电机、第二伺服电机和第三伺服电机分别驱动第一摄像装置上 下运动、绕光轴运动和左右运动，第四伺服电机、第五伺服电机和第六伺服电机分别驱动第 二摄像装置上下运动、绕光轴运动和左右运动。

[0068] 通过第一伺服电机、第二伺服电机和第三伺服电机反馈的位置信息获取左眼在三 个方向上的转角信息，包括左眼的光轴方向和X轴的夹角0 i、左眼的光轴方向和Y轴的夹 角、左眼的光轴方向和Z轴的夹角

[0069] 通过第四伺服电机、第五伺服电机和第六伺服电机反馈的位置信息获取右眼在三 个方向上的转角信息，包括右眼的光轴方向和X轴的夹角0 2'、右眼的光轴方向和Y轴的 夹角a 2、右眼的光轴方向和Z轴的夹角02。

[0070]步骤4，如图2所示，具体包括：

[0071] 步骤401，根据三角几何关系，得到：

[0072]

[0073] 步骤402,分别计算出目标特征点P与左眼的光心位置1之间的距离h、目标特征 点P与右眼的光心位置r之间的距离1 2;

[0074]

[0075] \ . 1 z.，

[0076] 其中，0，+ 92= Jr，b为光心位置1和光心位置r之间的距离。

[0077]步骤5，如图2所示，具体包括：

[0078]步骤501，根据三角几何关系，得到：

[0079] xx= 1 ^os 6 i；y i= 1 i〇〇s a 1；z x= 1 ^os 0

[0080] xr= 1 {os Q 2;yr= 1 {os a2;z r= 1 {os 旦2;

[0081] 步骤502,根据步骤4计算出的ljP12，计算得到：

[0082]

[0083]

[0084] 即目标特征点P在左眼坐标系下的坐标(Xi, y:，zD和在右眼坐标系下的坐标 (xr, yr, zr)分别为：

[0085]

[0086]

[0087]步骤6，如图2所示，步骤6具体包括：

[0088] 步骤601，假定目标特征点P在左眼中的图像和在右眼中的图像在同一个平面上， 则目标特征点P在左眼坐标系和在右眼坐标系下的y坐标相同，即：

[0089] y!= yr；

[0090] 步骤602,根据三角几何关系，得到：

[0091]

[0092] 其中，模拟左眼的第一摄像装置和模拟右眼的第二摄像装置的焦距均为f;

[0093] 步骤603,根据步骤602中的前两个计算式得到：

[0094]

[0095] 进而计算得到：

[0096]

[0097] 进而计算得到：

[0098]

[0099] 步骤604,记视差D=Xi-Xp根据步骤5中计算得到的坐标(Xi,y:，zD和坐标 (xr,yr,zr)，得到：

[0100]

一一n ^z，[0101] 步骤605,进一步计算得到：

[0102]

[0103]

[0104] ?----V~1-2/[0105] 即得到目标特征点P的深度信息为：

[0106]

[0107]

[0108] 本友明的双目视觉匹配算法通过视觉佝服使双目注视视觉目标的问一点，即该特 征点与双目中图像的光心位置重合，这时候利用伺服电机反馈的位置信息，通过三角形几 何法得出目标特征点相对于双目的深度信息。

[0109] 以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技 术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修 改、等同替换、改进等，均应包含在本发明的保护范围之内。

【主权项】

1. 一种双目视觉立体匹配算法，其特征在于，包括： 步骤1，左眼和右眼注视空间物体的同一目标特征点P，使所述目标特征点P在所述左 眼中的图像和在所述右眼中的图像分别与所述左眼的光心位置1和所述右眼的光心位置r 重合； 步骤2,建立左眼坐标系、右眼坐标系和世界坐标系； 步骤3,获取所述左眼和所述右眼在三个方向上的转角信息，包括所述左眼的光轴方向 和X轴的夹角S1、所述左眼的光轴方向和Y轴的夹角Ct1、所述左眼的光轴方向和Z轴的夹 角P 1、所述右眼的光轴方向和X轴的夹角、所述右眼的光轴方向和Y轴的夹角α2、所 述右眼的光轴方向和Z轴的夹角β 2; 步骤4,根据三角几何法，分别计算出所述目标特征点P与所述左眼的光心位置1之间 的距离I1、所述目标特征点P与所述右眼的光心位置r之间的距离I2;其中，θ 2' + θ 2= π，b为光心位置1和光心位置r之间的距离； 步骤5,分别计算出所述目标特征点P在所述左眼坐标系下的坐标(Xl，yi，Z1)和在所述 右眼坐标系下的坐标(Xpyp zj ;步骤6,根据双目视觉原理，得到所述目标特征点P在所述世界坐标系下的世界坐标 (Xw，Yw，Zw)，即得到所述目标特征点P的深度信息；其中，所述左眼和所述右眼的焦距均为f。2. 根据权利要求1所述的双目视觉立体匹配算法，其特征在于，步骤2中，所述左眼坐 标系是以所述左眼的光心位置1为原点建立的三维坐标系，所述右眼坐标系是以所述右眼 的光心位置r为原点建立的三维坐标系，所述世界坐标系是以所述左眼的光心位置和所述 右眼的光心位置之间的中点位置为原点建立的三维坐标系。3. 根据权利要求1所述的双目视觉立体匹配算法，其特征在于，步骤4具体包括： 步骤401，根据三角几何关系，得到：步骤402,计算得到4. 根据权利要求1所述的双目视觉立体匹配算法，其特征在于，步骤5具体包括： 步骤501，根据三角几何关系，得到：步骤502,根据步骤4计算出的IJP 1 2，计算得到：即所述目标特征点P在所述左眼坐标系下的坐标(Xl，yi，Z1)和在所述右眼坐标系下的 坐标Up y。Z1O分别为：5. 根据权利要求1所述的双目视觉立体匹配算法，其特征在于，步骤6具体包括： 步骤601，假定所述目标特征点P在所述左眼中的图像和在所述右眼中的图像在同一 个平面上，则所述目标特征点P在所述左眼坐标系和在所述右眼坐标系下的y坐标相同， 即： Yl= y r； 步骤602,根据三角几何关系，得到：步骤603,根据步骤602中的前两个计算式得到：进而计算得到：进而计算得到：步骤604,记视差D = X1-X1^根据步骤5中计算得到的坐标(X1, Y1, Z1)和坐标 (xr, yr, zr)，计算得到：步骤605,进一步计算得到：即得到所述目标特征点P的深度信息为：

【专利摘要】本发明公开了一种双目视觉立体匹配算法，包括：步骤1，左眼和右眼注视空间物体的同一目标特征点，使目标特征点在左眼中的图像和在右眼中的图像分别与左眼的光心位置和右眼的光心位置重合；步骤2，建立左眼坐标系、右眼坐标系和世界坐标系；步骤3，获取左眼和右眼在三个方向上的转角信息；步骤4，根据三角几何法，计算出目标特征点与左眼的光心位置之间的距离、目标特征点与右眼的光心位置之间的距离；步骤5，计算出目标特征点在左眼坐标系下的坐标和在右眼坐标系下的坐标；步骤6，根据双目视觉原理，得到目标特征点在世界坐标系下的世界坐标。本发明的有益效果为：算法简单可行，复杂程度大大降低，同时可满足三维定位精度的要求。

【IPC分类】H04N13/00, G06T7/00, G06T5/00

【公开号】CN105187812

【申请号】CN201510556591

【发明人】刘培志, 赵小川, 陈晓鹏, 孔晓梅, 施建昌

【申请人】中国兵器工业计算机应用技术研究所

【公开日】2015年12月23日

【申请日】2015年9月2日