基于仿真机器人的双目视觉定位*

周子啸 ，赵晓林 ，胡 峰 ，张 利

（清华大学 a.微纳电子系；b.电子工程系，北京 100084）

1 引言

计算机视觉是实现机器人人工智能的关键技术之一。很多视觉系统中，需要获得目标物体的位置信息，目标定位是计算机视觉的一个重要功能。借助图像信息的目标定位中很重要的一个实现方法是模仿人眼的视觉功能的双目视觉定位。

Marr教授首先提出了一种双目视觉计算方法，奠定了双目立体视觉发展的理论基础。双目视觉直接模拟人类双眼处理景物的方式，可靠简便，在许多领域极具应用价值。日本研制了一种自适应双目视觉伺服系统，利用双目立体视觉原理，实现对运动方式未知目标的自适应跟踪；日本东京大学将实时双目立体视觉和机器人整体姿态信息集成，开发了仿真机器人动态导航系统[1]。哈尔滨工业大学采用异构双目活动视觉系统实现了全自主足球机器人导航。火星“863”计划课题《人体三维尺寸的非接触测量》，采用双视点投影光栅三维测量原理，由双摄像机获取图像对，通过计算机进行图像数据处理，不仅可获取服装设计所需的特征尺寸，还可根据需要获取人体图像上任意一点的三维坐标[1]。

笔者在已搭建的实验平台上，实现了双目视觉的定位功能，该机器人平台的定位功能需求通过获取图像信息和机器人姿态信息，定位图像中各个匹配目标点的坐标，使原有系统功能得到提升。

2 机器人仿真实验平台

2.1 硬件系统

本系统的实验平台采用导轨机器人平台体制构建一个模拟仿真环境，通过搭建一个二维机器人导轨架结构，在获取运动信息和云台姿态的情况下，对机器人仿真环境下的目标进行检测、识别、跟踪和定位。该平台机器人硬件部分如图1所示。

其中，2个机械臂可在X方向上移动，在机械臂上分别有可在Y方向上移动的导轨机器人。摄像机搭载在2个Y轴导轨悬挂的高精度数控云台吊舱上，伺服电机驱动吊舱沿横向Y导轨运动，提供了摄像机的Y方向运动自由度。高精度数控云台能实现摄像机绕竖直向上的Z轴转动和在Z-Y平面、Z-X平面俯仰的精确运动，以仿真摄像机的各种拍摄姿态。机器人运动信息和云台运动信息均可以通过采集卡传输到计算机控制系统，实现对机器人和云台的精确控制。

2.2 软件系统

本系统的软件框架如图2所示，在该系统中完成双目定位功能模块。

3 基本原理

3.1 参数定位法

基于仿真平台的基本定位算法主要采用的是参数定位法和双目视觉定位法。参数定位法的基本原理是通过获取云台姿态信息参数，然后解算出摄像机视轴与理想地球球面的交点作为定位结果，参数定位法原理如图3所示。

其中，坐标系D，E，F表示通过解算求得的摄像机镜头坐标系，它的视场中某向量与地球表面的交点B即为定位结果。此参数定位法的优点是算法实现简单快速，但它是基于理想的地球椭球模型，如果需要更高精度的定位，需要有详细的地理表面信息库作为支持。本实验平台有2个导轨机器人，采用相似的定位算法，数据融合时，按它们距目标距离的权重作融合，并没有更充分地利用图像信息和2架云台之间的关系信息。

为了更充分地利用2个导轨移动机器人的图像信息，只要云台不处于某些特定的姿态，如两摄像头的视轴正好平行（实际中只在极少情况下出现这种姿态），可以将两摄像头的图像信息作双目视觉定位，充分利用多机通信协同定位，更好地利用图像信息，也很少需要地理信息库等其他条件的支持。在实际系统应用中，可以使用参数定位法与双目视觉法分别求解，为定位结果提供更多的参考基础。

3.2 单目视觉原理

单目视觉的基本原理如图4所示。

由摄像机针孔成像模型可以得到相似关系

式中：（x，y）为图像中 P 点的坐标，（X，Y，Z）为空间中 P 点的坐标，f为镜头的焦距。在已知焦距f的情况下，空间中的点可以与图像中的点的坐标值相互转化。

由空间中的坐标点映射为图像中坐标点的公式为

式（3）为式（2）的展开形式，矩阵A为摄像机的内参数矩阵，fx和fy分别表示径向和轴向焦距，cx和cy表示主点（Principle Point）的坐标；矩阵R为摄像机镜头坐标系在世界坐标系旋转矩阵，T为摄像机镜头坐标系在世界坐标系中的平移向量；W表示P点的世界坐标，在展开形式中以齐次坐标表示；R和T形成的矩阵称为外参数矩阵，内参数矩阵与外参数矩阵的乘积定义为矩阵M。若同时有两个摄像机的多个对应点，即可形成双目视觉的情况，如图5所示。

若该世界坐标系中的同一点P在左右两摄像机中分别形成点P1和P2，则有映射方程

将式（4）、（5）分别展开并联立方程

解此方程组即可求得世界坐标系下的坐标值。

4 算法实现

整个算法实现的流程如图6所示。

图6 算法实现流程

4.1 摄像机标定

求解摄像机镜头内参数与外参数的过程称为摄像机标定。内参数是定位过程中求解必要的参数，在摄像机镜头焦距等内参数情况改变时必须重新进行，由于仿真平台使用的是定焦镜头，所以每次调焦后只需进行一次标定即可。标定算法采用的是基于张正友标定算法[2]实现的Calibration Toolbox[3-4]。为了便于角点检测，选取若干幅棋盘格平面图进行标定，如图7所示。

由于机器人摄像机是定焦镜头，内参数在镜头不变焦的情况下是不变的，所以由此标定算法求出的内参数将在机器人系统中作为初始配置参数。

由此标定算法求出的内参数与外参数均作为输入参数完成一次仿真验证，用以确定双目定位算法的正确性，并且为机器人平台调试提供参考真值。

4.2 Matlab仿真验证

由Calibration Toolbox解算出的内参数与外参数进行仿真实验。在多个视图中分别手工依次标定多个目标点，本次实验取的4个点是图7中棋盘格外圈的4个内角点，然后将这16幅图片依次作为左右视角图片，进行双目视觉定位解算，结果如图8所示。

图7中每一个小格是0.02 m，取左上角点为原点，则定位所得的结果坐标应为（0，0），（0，0.14），（0，0.1）和（0.14，0.10）4个值。每一个点由16幅图两两定位，即每一个点有120个双目定位结果，由图8可以定性地看出结果的正确性，只有极少几个点的值与定位结果相差较大。

4.3 仿真机器人平台实现及试验结果

已经实现的软件可以完成目标的跟踪、识别和基于参数法的定位功能。为了有效利用数据，实现多机通信的协同定位，本系统添加了双目视觉定位模块。该模块主要包括双目视觉算法子模块与系统接口子模块2个部分。

定位算法子模块完成求解方程组以解算目标坐标值的功能；接口子模块负责与其余模块通信，获取姿态参数，将姿态参数转化成外参数矩阵，与初始配置的内参数矩阵一起作为定位的输入参数。

在VC++开发环境下编写的程序嵌入系统，完成该双目定位功能，试验结果如表1所示。

表1 定位坐标与真值

定位结果误差如图9所示。

由上述定位结果误差分析可以看出，定位结果的解算精确度与定位点的位置有关。由于边角点离图像中心主点比较远，畸变相对于处于图像中心点的原点位置大，定位结果误差较大，而且定位结果误差主要来自Z轴方向的空间点计算误差，由于Z轴方向的数值完全由摄像机镜头的焦距解算得到，图像模糊畸变对于结果影响较大，若已知目标的水平高度，仅计算在X-Y平面内的偏离直线长度，屏蔽Z轴的误差，误差均在10 mm以内，定位结果的准确性明显提升。

5 小结

介绍了双目视觉的基本原理，基于此原理开发出在仿真机器人平台上的双目视觉定位功能，使基于参数法定位的原机器人系统的定位功能得到了提升。由定位结果可以看出，定位结果准确，并且有较高的精度。

[1]林琳.机器人双目视觉定位技术研究[D].西安：西安电子科技大学,2009.

[2]ZHANG Z Y.Flexible camera calibration by viewing a plane from unknown orientations[C]//Proc.of IEEE International Conference on Computer Vision.Corfu， Greece:[s.n.]， 1999： 666-673.

[3]BOUGUET J Y.Camera calibration toolbox for Matlab[EB/OL].[2010-03-01].http：//www.vision.caltech.edu/bouguetj/calib_doc/.

[4]DAVID A F，JEAN P.Computer vision：a modern approach[M].New Jersey：Prentice Hall， 2004.