基于相机角度自适应的视觉伺服

唐明君，孔庆生

（复旦大学 电子工程系，上海200433）

0 引 言

视觉伺服是一种通过采集真实物体的图像，利用图像反馈的信息来控制或调整机器人的方法。一般来说，视觉伺服控制主要可以分为3类：基于图像的 （imagebased）［1，2］，基于位置的 （position－based）［3，4］和混合类型的（combined）［5，6］。但是以上3种类型的视觉伺服都离不开大量的图像处理，因此图像处理的速度成为限制视觉伺服系统实时性的最大瓶颈［7］。同时，装置传统相机的机器人由于视野域有限，可能丢失目标物体的特征点。为解决机器人装置传统相机角度固定造成的视野域有限的问题，参考文献 ［8，9］提出了采用全景相机的视觉伺服方法，然而视野的扩大也带来图像处理时计算量的成倍增加［10］。

以往大多数的视觉伺服控制主要集中在相机的路径轨迹设计，而忽视了相机角度的旋转因素。本文提出了基于给定轨迹的相机角度自适应调整视觉伺服算法。该算法通过建立相机坐标系和世界坐标系之间的映射关系，计算机器人运动轨迹上相近两点相机方向的角度差，自适应的控制相机旋转。因为加入了相机的自适应旋转，能够控制相机拍摄目标物体的特征点，使目标物体始终出现在相机的视野域中。

1 几何模型

视觉伺服类似于人类通过眼睛和大脑来获取和处理图像信息，然后控制身体运动。随着数字信号理论的发展和计算机硬件后摩尔时代的到来，成倍增长的计算能力和成本的降低使得通过计算机对视觉信号进行实时处理成为可能，这也推动了基于图像的视觉伺服的发展。采用全景相机的视觉伺服方法，虽然解决了机器人搭载固定传统相机造成的视野域有限问题，但随着视野的扩大，相应也带来了图像处理时计算量的增长。如图1所示，假设图像的清晰度一致，传统相机的视野角度为α，则采用全景相机的图像处理复杂度大致为传统相机的360／α（图像的高度相同，但宽度不同），但它们的有效观测域几乎相同。而基于图像和混合控制的视觉伺服计算量主要集中于图像处理模块，因此，减小图像的输入规模就能有效的提高视觉伺服控制的响应速度。本文采用可自由控制相机转动的角度自适应算法，可以显著降低计算复杂度，提高机器人移动控制的实时性，而且有限视野域的自由转动模型更接近人类处理视觉信息的过程。

图1 两种不同相机的视野域与计算量对比

类似于人类双眼的双目视觉伺服采用有角度差的两幅对特征点的图像作为输入信号，使用视觉差原理重建三维环境和目标物体的位置和几何信息。这两幅图像可以通过两个不同角度的摄像机或者单个相机在不同位置对同一特征点拍摄得到。本文采用角度可调的单目相机，假定相机的旋转和机器人的平移独立控制，不相互关联的。

不失一般性，假定每个被观测的物体有4个共面但不共线的特征点Oi（i＝1，2，3，4）［11］。如图2所示，假设机器人初始位置在c，以c为坐标原点建立直角坐标系，则特征点Oi在欧氏空间c中的坐标为ai＝ （aix，aiy，aiz）T；在短暂的时间Δt后，机器人位置在c′，以c′为坐标原点建立直角坐标系，则特征点Oi在欧氏空间c′中的坐标为a′i＝ （a′ix，a′iy，a′iz）T。Δt的时间足够短，可以假定特征点Oi的位置信息不变。我们可以通过Oi在两坐标系中的坐标的关系来计算坐标系的变换，包括平移和旋转。

图2 不同相机坐标系的坐标变换

图2可以看出直角坐标系c′可以由直角坐标系c经过平移T和旋转R得出。平移T使得c′的坐标原点和c的坐标原点重合，旋转使得两坐标系的xyz轴重合。可写作方程 （1）

其中R∈R3×3是一个3×3的矩阵，T∈R3是一个三维向量。

ai是Oi在以相机位置为坐标原点建立的世界坐标系中的坐标，是真实的三维空间的距离，ei是Oi在相机坐标系中的坐标，是图像测得的距离，可以通过相机采集的图像计算得出，也就是视觉伺服中控制模块的输入信息。真实距离ai和图像像素距离ei之间存在一个全局的可逆对应关系［12］：

其中M∈R3×3是常数的，相机固有的，上三角可逆矩阵

在方程 （3）中，u0，v0∈R 是主点 （principal point）（例如图象中心的像素点）在图象空间中的坐标。α，β∈R分别表示相机的像素和焦距长度。∈R表示相机轴线之间的夹角。总之，M是一个与相机参数相关的常数矩阵，相机校准后，M即为已知条件。由于M可逆，所以ai可以由ei计算得到

由方程 （1）（2）（3）可得

设E ＝ ［e1，e2，e3，e4］T，E＇＝ ［e′1，e′2，e′3，e′4］T，其中E，E′∈R4×3由 （5）可得

由于 （ei，e′i）可以由相机采集，视为已知，而按照假设机器人的路径已经规划好，即T已知，求R，这是一个超定方程。Oi（i＝1，2，3，4）是4个共面但不共线的特征点，由三点决定一个平面得出O4可以用O1，O2，O3线性表示，即矩阵E的秩为3，所以方程 （6）一定有解或最小二乘解。但由于测量误差，方程 （6）可能没有精确解。采用最小二乘解作为方程的近似解

2 控制模型设计

2.1 旋转矩阵的极坐标表示

目前的机器人视觉伺服中，机器人靠内置的发动机控制相机的转动，而发动机的输入参数是相机需要旋转的角速度 （时间Δt内变化的角度），故计算出旋转矩阵R后使用极坐标表示，便于计算相机旋转的角速度。

不妨设相机的初始方向与x轴单位向量n的方向一致，经过旋转相机的目标方向op的方向一致。由极坐标的知识可知，三维欧几里德空间R3中的任意点都可以由三维坐标（θ1，θ2，r）表示出。其中θ1∈ ［－π，π），θ2∈ ［－），r∈［0，＋∞）。旋转矩阵R作用于向量n的效果等同于将向量n先进行水平旋转θ1度，再进行垂直旋转θ2度。

由于旋转矩阵R只包含角度信息，故传统极坐标中的极半径r可以忽略，如方程 （8）

其中θ1∈ ［－π，π），θ2∈ ［－）。

如图3所示，可以计算出旋转的角度θ1和θ2，其中op是与x轴同向的单位向量（1，0，0）T经过旋转矩阵R的作用后得到的，即op＝（1，0，0）TR。p′是点p在xoy平面的投影，θ1是op′和x轴的夹角，即θ1＝∠p′ox。θ2是op′和op的夹角，即θ2＝ ∠p′op。

图3 R的极坐标表示

考虑平面机器人始终在同一平面运动，可以忽略垂直角度θ2的调整，只需要对角度θ1的进行自适应调整。可表示如方程

其中θ0∈ ［－π），是个常数，通常等于0。

因此R和θ1存在一一对应的关系。θ1可以由R表示为方程 （10）

这个简化使得平面机器人的相机只需要在一个维度上进行旋转，极大地减小了机器人控制发动机的难度，同时也提高了旋转精度，降低了计算的复杂度。根据上述R的极坐标表示法，可以设计角度自适应的机器人视觉伺服模型。

2.2 开环误差模型

在上述机器人控制相机旋转的模型基础上，可以建立相应的相机角度自动调整的开环误差视觉伺服模型。开环系统因为不需要构建输出到输入的反馈回路，使得系统的设计更加简单，计算的复杂度也会降低，符合机器人视觉伺服对实时性的要求。但系统的稳定性也可能随之减小，需要仔细地论证，本文将在之后讨论该系统的稳定性。

图4 开环系统及稳定性分析

如图4所示，假定ω∈R3是初始位置c的相机的方向向量，ω′∈R3是机器人移动后的位置期望c′的相机方向向量，角速度可定义为方程 （11）

式中：Δt——机器人从位置c移动到c′的时间，不妨设Δt为机器人调整发动机的单位时间。设ω＝ ［x0，y0，z0］T，其中＝1，则可以计算出ω′＝ωR。归一化后记作ω′ ＝ ［x′0，y′0，z′0］T，其中x＝1。由此可以计算出在Δt时间内需要转过的角度

方程 （12）中，x0和y0是初始位置的方向信息，为已知量。计算x′0和y′0需要用到的矩阵R可以由方程 （7）计算得出。因此，可以计算出机器人需要旋转的相机角度Δθ1。

2.3 稳定性

在开环误差系统中，如果误差无界或者累积误差不收敛，则系统的稳定性很差，严重时，可能给系统带来灾难性的崩溃。本文提出的开环误差系统误差有界且不累积，所以稳定性较好。

如图4所示，期望的旋转后的方向向量ω′和旋转后实际的方向向量Rω存在一个误差值，这是由测量误差造成的，可以表示为方程 （13）

根据测量误差有界和最小二乘法可以得出珘ω也是有界的。由方程 （12）可知，相应的转化成旋转角度误差珓θ也是有界的。系统根据实时图像计算旋转角度，误差并不累积。所以累积误差可记为珓θΣ＝Σ珓θ＝珓θn，珓θn为最后一次调整的误差。当t→∞时，珓θ（t）＜θM。θM由相机的参数决定的测量误差等决定。

3 实验结果

在Matlab中进行机器人视觉伺服控制算法的仿真。试验中使用的校准相机参数矩阵值为

图5为目标物物体在机器人相机图像空间中的运动轨迹图。o为开始位置，＊为结束位置。图6为机器人相机方向向量表示为极坐标后θ1随时间变化的曲线图像，初始角度设为0°。图7为期望调整角度与实际调整角度的误差珓θ随时间变化曲线图。从图像上可以看出珓θ＜5°，且不随时间累积。

4 结束语

本文采用机器人搭载的校准相机采集的图像作为输入，提出了相机角度自适应调整的视觉伺服算法，能够有效地解决移动机器人在视觉伺服控制过程中的视野域问题。由于采用了自适应的方法来调整传统相机的方向，而不同于全景相机增大输入图像的规模，因此既能保证目标物体始终处于相机的有效视野域，又不会增加图像处理的计算量，提高了视觉伺服控制系统的实时性。同时，由于系统误差不累积，使得系统能有较好的稳定性。

：

［1］Lippiello V，Sieiliano B Position.Based visual servoing in industrial multirobot cells using a hybrid camera configuration ［J］.IEEE Trans Robot，2007，23 （1）：73－86.

［2］Heitzmann T，Doignon C，Albitar C，et al.Position based visual servoing using a coded structured light sensor ［C］／／IEEE Conf Robotic and Sensors Environments，2008：126－131.

［3］Chaumette F，Hutchinson S，Visual servo control I.Basic approaches ［M］.IEEE Robot Autom，2006，13 （4）：82－90.

［4］Mariottini G L，Oriolo G，Prattiehizzo D.Image based visual servoing for nonholonomic mobile robots using eplpolar geometry ［J］.IEEE Trans Robot，2007，23 （1）：87－100.

［5］HU Guoqiang，Nicholas Gans，Norman Fitz Coy，et al.Adaptive homography－based visual servo tracking control via a quaternion formulation ［J］.IEEE Trans Controls Systems Technology，2010，18 （1）：128－135.

［6］Heitzmann T，Doignon C，Albitar C，et al.Position based visual servoing using a coded structured light sensor ［C］／／Robotic and Sensors Environments，2008：126－131.

［7］Novotny P M，Stoll J A，Dupont P E，et al.Real time visual servoing of a robot using three dimensional ultrasound ［C］／／IEEE Conf Robotics and Automation，2007：2655－2660.

［8］Hadj Abdelkader H，Mezouar Y，Martinet P，et al.Catadioptric visual servoing from 3Dstraight lines ［J］.IEEE Trans Robotics，2008：652－665.

［9］YANG Shaoping，CHEN Xiong，KONG Qingsheng.Imagebased visual servoing for mobile robots with central panoramic camera ［J］.Computer Engineering and Design，2010，31（19）：4261－4264 （in Chinese）. ［杨少平，陈雄，孔庆生.采用全景相机的移动机器人视觉伺服 ［J］.计算机工程与设计，2010，31 （19）：4261－4264.］

［10］Tahri O，Araujo H.Non central catadioptric cameras visual servoing for mobile robots using a radial camera model ［C］／／Intelligent Robots and Systems，IEEE／RSJ International Conf，2012：1683－1688.

［11］Chaumette F，Hutchinson S.Visual servo control II.Advanced approaches［Tutorial］［C］／／Robotics ＆ Automation Magazine，IEEE，2007：109－118.

［12］Mariottini G L，Oriolo G，Prattichizzo D.Image based visual servoing for nonholonomic mobile robots using epipolar geometry ［J］.IEEE Trans Robotics，2007 （23）：87－100.