基于单应性矩阵分解的视觉控制器设计及仿真

宗晓萍，李悦霞，刘伟东

（河北大学电子信息工程学院，河北保定 071002）

基于单应性矩阵分解的视觉控制器设计及仿真

宗晓萍，李悦霞，刘伟东

（河北大学电子信息工程学院，河北保定 071002）

机器人视觉伺服是指利用视觉传感器得到的图像信息，构造机器人的位置闭环反馈，从而实现机器人的定位控制或者轨迹跟踪.根据视觉反馈信息的类型，机器人视觉伺服分为基于位置的视觉伺服控制和基于图像的视觉伺服控制［1］.随着图像雅克比矩阵的引入，基于图像的视觉伺服控制方法（IBVS）在20世纪80年代中期兴起.该方法计算高效，并且对空间模型和摄像机标定误差具有鲁棒性.继而研究者提出了各种各样对图像雅克比矩阵进行线性化的方法［2］，但这些研究仅限于机器人末端执行器或目标做微小运动的情况.同时，Chaumette等提出了若干不能应用传统局部线性化方法来解决的视觉伺服控制问题［3］.随后Ezio Malis等提出了基于单应性矩阵分解的视觉伺服控制方法［4］，许多学者就单应性矩阵的分解算法进行研究［5］.

本文视觉伺服系统采用双环动态look－and－move的控制方式［1］，基于图像的机器人视觉伺服系统结构如图1所示，摄像机采用单目眼在手的安装方式（eye－in－hand），内环为关节伺服控制，外环为视觉控制.视觉控制器采用应用广泛的Faugera和Lustman的单应性矩阵分解方法，但该方法最终会得到2组不同的解［5］，而只有1组解能够使得机器人完成视觉伺服任务.在此，采用中间图像插值方法，根据摄像机位姿间的关系对分解的结果进行筛选，从而得到正确的解，进而提取RPY角得到机器人的控制量，并通过实验验证了该方法的有效性.

图1 基于图像的机器人视觉伺服结构Fig.1 Structure of robot visual servo based on image

1 成像平面间的单应性关系

当所观察的目标特征点都在同一个平面上时，摄像机在不同时刻对该平面所成图像间的关系就是直射变换，即单应性（Homography）［5］.2个成像平面间的几何关系如图2所示，设空间一平面π，M为该平面上一点，n为平面π的单位法向量，F1，F2分别是摄像机处于当前和期望位置时的摄像机坐标系，π1，π2是相应的成像平面.设点M在F1中的坐标为χ＝［XCYCZC］，在成像平面π1中的齐次坐标为m＝［xy1］.当摄像机到达期望位置时，点M的摄像机坐标、成像坐标分别用χ＊，m＊表示.

图2 2个成像平面间的几何关系Fig.2 Geometric relationship between the two retinals

摄像机采用针孔模型，对焦距进行归一化，则摄像机坐标与成像坐标间的关系为

此时，设矩阵A是点M在2个成像平面上射影点间的单应性矩阵，则

2 视觉控制器的设计

本文的视觉控制器正是利用成像平面间的单应性关系，通过分解单应性矩阵得到机器人的运动参数，建立起图像特征点与机器人位姿之间的关系，从而完成视觉伺服任务.

2.1 求解单应性矩阵

至少需要4对相互独立的射影点才能唯一确定一个直射变换.由于成像坐标采用第3个元素为1的齐次坐标，因而，令（2）式中A的元素a33＝1，以第1对射影匹配点为例［6］，代入（2）式，可得

对（3）式进行整理，可得

对于2个成像平面中的每对匹配点，都能得到一组如（4）式所示的方程.4对独立的射影点就能得到含有8个未知量aij（i＝1，2，3，j＝1，2，3，除a33以外）的线性方程组，如（5）式所示.

如果问一问和今天的教育相匹配的词语是“快乐”还是“沉重”，是“成长”还是“升学”，我想大多数人都会选择后者。是的，正是现行的教育，让无数个家庭、家长和孩子面临沉重的压力：择校压力、升学压力、考试压力、分数压力……并因此焦虑愁苦、忧心如焚。正是这样的教育，让社会上充斥着各种追名逐利的办学机构，培优班门庭若市，小考、中考、高考辅导班挤破脑袋……正是这样的教育，让学校里满是学生肩膀上轻不下来的书包，教师脸上挥之不去的愁容，校长心里落不下地的石头……显然，这样的教育，只会让无数人指责和诟病，呼吁和请求，不能让人满意。

通过求解线性方程组（5）式，就能求得单应性矩阵A.

2.2 分解单应性矩阵

设d为平面π到当前摄像机坐标系原点处的距离，则平面π的方程为

设R为3×3的旋转矩阵，t为3维的平移向量，它们描述2个摄像机坐标系间的关系，则

由（6）式和（7）式，得

由（1）式知，χ和χ＊分别是m和m＊的射影坐标，从而可得

根据文献［5］，分解单应性矩阵A，从而得到摄像机运动和特征点所在平面的参数，即R，t，n和d.一般情况下，方程（9）可得到8组解，然后通过摄像机可见性进行约束，筛选得到2组可以物理实现的解［5］.

由文献［5］对单应性矩阵进行分解所得的2组解差别很大，事实上，只有其中正确的一组解才能使摄像机由当前位置顺利地到达期望位置［7］.

采用中间图像插值的方法对分解结果进行进一步地筛选，即：给出当摄像机处于当前位置与期望位置间的一个中间位置时对特征点所成的图像，设π3是摄像机在中间位置时的成像平面，点M在该平面的成像坐标为m′＝［x′y′1］.同时设中间摄像机坐标系相对于期望摄像机坐标系的旋转矩阵为DRM，当前摄像机坐标系相对于中间摄像机坐标系和期望摄像机坐标系的旋转矩阵分别为MRC和DRC.由于摄像机的期望位置是一定的，而所插入的中间位置由自己选定，因而DRM为已知量.这时利用m和m′，m和m＊能够计算出2个相应的单应性矩阵，并且按照文献［5］对其进行分解，得到MRC和DRC各2组解，它们需满足摄像机位置之间的关系

通过排列组合的形式能够找出满足（10）式的唯一解，以完成视觉伺服任务.

2.4 提取RPY角

选出正确的解后，需要从旋转矩阵R中提取出滚动角φr（roll）、俯仰角φp（pitch）和偏航角φy（yaw）.点M由当前摄像机坐标到期望摄像机坐标的3×3旋转矩阵为R，设以RPY角表示的旋转矩阵为Q＝Rot（x，φr）Rot（y，φr）Rot（z，φy），如果只考虑旋转运动，可知R＝Q，则

控制器的输出部分：以单应性矩阵分解结果中的t作为摄像机由当前位置到达期望位置的平移速度，即T＝［TxTyTz］＝t，使用R中提取的RPY角作为角速度［8］，即Ω＝［ωxωyωz］＝［φrφpφy］，因而可得到视觉控制器的控制输出为y＝k［TxTyTzωxωyωz］，k为增益系数.

3 仿真实验

3.1 构建仿真模型

以Puma560机器人为例，利用Matlab中的机器人工具箱和机器视觉工具箱，在Simulink环境下创建sub－system来构建视觉伺服系统仿真模型.基于单应性矩阵分解的视觉伺服仿真结构如图3所示.

图3 基于单应性矩阵分解的视觉伺服系统仿真结构Fig.3 Simulation structure of the visual servo system based on homography matrix decomposition

系统仿真模型由以下3个部分组成：

1）机器人部分由jacobn模块、ijacob模块、相乘（Product）模块、积分（Integrator）模块和fkine模块组成［9］.jacobn模块中J＝jacobn（robot，q）可以求得机器人雅克比矩阵，其中robot选用6自由度的关节式机器人Puma560，q为机器人的关节角向量.ijacob模块完成矩阵求逆运算.相乘模块实现q·＝J－1p·，p为机器人的位姿，从而得到关节角的变化量q·.然后通过T＝fkine（robot，q）求得机器人总的变换阵.

2）摄像机模型实现三维场景到二维场景的转换.Camera模块中uv＝camera（p，C，T），其中p为目标特征点的3维世界坐标，C为摄像机的内参矩阵，T为摄像机坐标系相对于世界坐标系的变换矩阵，模块输出uv是目标特征点在当前摄像机成像平面内的图像像素坐标.

3）视觉控制器由单应性控制器和比例环节构成.单应性控制器采用子系统封装的形式，首先根据摄像机针孔模型完成特征点像素坐标到成像坐标的转换，然后，应用该文中所提出的方法求解并分解单应性矩阵，应用中间图像插值筛选出正确的一组解，分别输出平移向量t和旋转矩阵R，最后根据公式（11）从旋转矩阵R中提取RPY角.

3.2 仿真实验验证

本实验以长方体为跟踪目标，选取长方体上表面的4个角点作为特征点进行定位.这4个特征点的世界坐标分别为［2，0.3，0.2］m，［2，0.3，－0.2］m，［2，－0.3，－0.2］m，［2，－0.3，0.2］m.摄像机配置如下：焦距为8mm，物理感光阵列为6.4mm×6.4mm，像素为512×512，其成像平面的原点在图像平面的坐标为［256，256］pixel.目标特征点的期望像素坐标为［256，406］pixel，［456，406］pixel，［456，106］pixel，［256，106］pixel.

目标特征点在图像平面的移动轨迹如图4所示，其中‘＊’表示特征点的起始位置，‘＋’表示特征点的期望位置，‘o’表示特征点实际到达的位置，可以看出采用基于单应性矩阵分解的视觉控制器能够使目标特征点成像于期望位置.图5表明目标特征点的像素误差逐渐趋近于零，说明采用该视觉控制器能够顺利地完成机器人的视觉伺服任务.

图4 目标特征点在图像平面的移动轨迹Fig.4 Movement trajectory of the target feature points in the image plane

图5 目标特征点的图像像素误差的均方根曲线Fig.5 Root mean square curve of image pixel error for the target feature points

4 结论

设计了一种基于单应性矩阵分解的视觉伺服控制器，充分利用了摄像机在不同时刻对目标物体所成图像间的单应性关系，通过分解单应性矩阵，进而得到机器人的运动参数.该方法避免了图像深度信息的求取，而且它能够满足机器人的一般运动，因而在一定程度上拓宽了机器人的应用范围.最后，仿真实验表明，应用该方法设计视觉控制器，目标特征点的图像像素误差趋近于零，机器人能够快速准确地到达期望位置.

［1］ HUTCHINSON S，HAGER G D，CORKE P I.A tutorial on visual servo control［J］.IEEE Trans on Robotics and Automation，1996，12（5）：651－670.

［2］ PAPANIKOLOPOULOS N P，KHOSLA P K，KANADE T.Visual tracking of a moving target by a camera mounted on a robot：a combination of vision and control［J］.IEEE Trans on Robotics and Automation，1993，9（1）：14－35.

［3］ CHAUMETTE F.Potential problems of stability and convergence in image－based and position－based visual servoing［C］／／KRIEGMAN D，HAGER G，MORSE S，et al.The confluence of vision and control，Lecture Notes in Cont and lnfo Sci，Berlin：Springer－Verlag，1998：66－78.

［4］ MALIS E，CHAUMETTE F，BOUDET S.2－1／2Dvisual servoing［J］.IEEE Trans on Robotics and Automation，1999，15（2）：238－250.

［5］ FAUGERAS O，LUSTMAN F.Motion and structure from motion in apiecewise planar environment［J］.International Journal of Pattern Recognition and Artificial Intelligence，1988，2（3）：485－508.

［6］ 宗晓萍.智能机器人视觉伺服系统研究［D］.保定：河北大学，2007.

ZONG Xiaoping.Study of the visual servoing system on intelligent robot［D］.Baoding：Heibei University，2007.

［7］ 班建安.机器人混合视觉伺服控制研究［D］.西安：西安理工大学，2009.

BAN Jianan.Research on robot hybrid visual servoing control［D］.Xi'an：Xi'an University of Technology，2009.

［8］ GANS N，HUTCHINSON S.A switching approach to visual servo control［J］.Proceedings of the 2002IEEE International Symposium on Intelligent Control，2002，2002：770－776.

［9］ 宗晓萍，淮小利，王培光.基于图像的PUMA560机器人视觉伺服系统仿真［J］.机床与液压，2007，35（10）：161－164.

ZONG Xiaoping，HUAI Xiaoli，WANG Peiguang.Simulation of image－based visual servoing system for PUMA560robot［J］.Machine Tool ＆Hydraulics，2007，35（10）：161－164.

（责任编辑：孟素兰）

Design and simulation of visual controller based on homography matrix decomposition

ZONG Xiaoping，Ll Yuexia，LlU Weidong
（College of Electronic and Informational Engineering，Hebei University，Baoding 071002，China）

In order to avoid solving the depth information in the traditional visual servo control method based on image－jacobian matrix，and overcome the constraint involved only small motions of robot end－effector，a visual controller based on homography matrix decomposition was designed in this paper.According to the decomposition of homography matrix between the two retinas when a camera forms the image of aplane at different time，and the screening method of intermediate image interpolation，the visual controller parameters were determined.Finally，the system simulation for Puma560was implemented in the Matlab／Simulink，and the experimental results verified the validity of the method.

visual servo control；homography matrix；image interpolation；Puma560

为避免传统的基于图像雅克比矩阵视觉伺服控制方法中深度信息的求取，克服机械手末端仅限于做微小运动的约束，设计了基于单应性矩阵分解的视觉控制器.根据对摄像机在不同时刻对同一平面所成图像间的单应性矩阵的分解，并通过中间图像插值筛选方法，确定其视觉控制器参数.最后，以Puma560机器人为例在Matlab／Simulink环境下进行系统仿真，其结果验证了该方法的有效性.

视觉伺服控制；单应性矩阵；图像插值；Puma560

10.3969／j.issn.1000－1565.2013.03.014

TP24

A

1000－1565（2013）03－0300－05

2012－12－20

国家自然科学基金资助项目（面上项目）（11271106）

宗晓萍（1964－），女，河北蔚县人，河北大学教授，博士，主要从事混合动态系统及智能控制方面的研究.

E－mail：zxp＠hbu.edu.cn