基于视觉反馈的机械臂预测控制

杨马英，郑亚飞

(浙江工业大学 信息工程学院，浙江 杭州 310023)

基于视觉反馈的机械臂预测控制

杨马英，郑亚飞

(浙江工业大学 信息工程学院，浙江 杭州 310023)

摘要：针对基于视觉的机器人目标抓取问题，构建一个基于图像的单目视觉伺服系统.预先建立基于图像雅克比矩阵的模型，在明确考虑目标可见性约束及机器人执行器约束的前提下，将预测控制算法引入到机械臂的视觉伺服控制中，基于视觉特征的运动预测设计一个简化的多变量机械臂视觉伺服控制器，用以实现对机械臂末端位姿的控制.借助NAO机器人平台，利用机器人自带的Naomark标签进行物体的快速识别，并应用上述基于视觉反馈的机械臂预测控制方法实现NAO机器人手臂控制.实验结果验证了方法的可行性，算法简单易于实现，且控制精度令人满意.

关键词：视觉伺服；图像雅克比矩阵；预测控制；NAO机器人

机械臂视觉伺服[1-5]是将视觉传感器作为感知机构，实时采集目标物与机械臂末端效应器的图像信息，通过图像预处理、特征提取等环节获得机械臂工作环境的信息反馈，构建闭环控制，驱动机械臂运动到指定位姿.视觉伺服使机器人更智能化，增强对外界环境的适应性，在众多应用领域得到了广泛的关注，如农业采摘机器人[6].在伺服控制算法上有经典控制方法，如反馈比例控制器，该方法易于实现，但忽略了机器人运动学及动力学限制，这类约束对控制性能却有显著影响，当系统初始位姿与期望位姿之间存有较大位移时，由于系统可见性约束或者执行机构的机械约束，造成视觉伺服任务失败[7-9].智能控制方法也被应用于机器人视觉伺服研究，如神经网络[10]、模糊控制[11-12]以及预测控制[13-15]等.神经网络需预先进行离线训练，环境变化时则重新训练, 限制了其应用.模糊控制利用模糊规则描述视觉伺服系统中各变量之间的关系，不需要精确的系统模型, 但需一定的先验知识或离线学习.对于多自由度视觉伺服系统, 变量之间关系复杂且耦合严重, 模糊规则的设计比较困难.预测控制的模型不要求严谨的结构形式，只强调其预测功能,预测控制策略虽发端于工业过程控制，也适用于机械臂控制[16-17].Lazar等用非线性模型预测控制设计视觉控制器，考虑可见性约束，通过非线性约束最优问题实现了机器人运动的收敛[18].

因预测控制对机械臂模型要求不高,有通过DH运动学参数确定机械臂运动学模型，运用运动学逆解求解得相对应的关节角变量，但运动学逆解运算量大且复杂.也有利用拉格朗日方程建立机械臂动力学方程，但机械臂动力学方程是时变的强非线性方程，计算量大.在一定的操作空间内，图像雅可比矩阵可直接将机器人在工作空间的速度变化映射为目标在图像特征参数空间的变化，使机器人视觉伺服不再受复杂的机器人和摄像机系统建模限制，只要求出这种映射关系就可以通过视觉特征误差计算机器人控制量，实现机器人控制.因此，在明确考虑系统约束问题前提下，提出简化的基于视觉反馈的机械臂预测控制方法.采用基于图像雅克比矩阵的预测模型，设计预测控制器，其中控制器的设定和测量反馈均是视觉特征位置量，考虑目标可见性约束及机器人执行器约束，通过在线滚动优化计算机械臂的关节角运动控制量，驱使机械臂末端向指定位姿运动.最后在NAO机器人实验平台进行验证，实验结果表明了该方法的控制可行性，且计算相对简单，易于实现.

1预测控制算法的实现

1.1预测模型的建立

对于任意一个实际控制系统，好的模型可以在一定程度上简化控制器设计的复杂度，从而提高系统控制性能.因此，为了描述机器人手眼映射关系，需要选择一种比较合适的模型.图像处理与定位是视觉伺服控制系统的重要组成部分，近年已有很多研究成果[19].因而不再详细讨论图像特征提取技术，而是在此基础上选择经典视觉伺服控制内部交互矩阵作为预测模型.通过模型，可以在有限的预测域内预测得到视觉特征相对于摄像机移动速度的变化率.

经典内部模型特征点速度与输入的关系表达式为

(1)

假设预测控制中每次迭代的视觉特征取样周期为Te,离散后得

Np-1)Tev(k+Np-1|k)

(2)

其中：Jp为雅克比矩阵；sm(k)为图像在k时刻的视觉特征点；v(k)为机械臂的运动矢量；Np为模型预测的时域范围.式(2)为图像特征迭代的一步预测模型.

解析法、在线估计法和离线学习法是雅克比矩阵最常用的求解方法.利用解析法求解图像雅克比矩阵,需要对摄像机进行标定,而且需要求取机器人的模型.在线估计法与初始值的选取密切相关.离线学习不要求对摄像机内部参数和机器人参数进行标定,但要求进行离线学习.

通过预先求取图像雅克比矩阵，在实现视觉伺服任务过程中保持图像雅克比矩阵不变.该方法只需计算一次图像雅克比矩阵，较为简单.该方法适用于摄像机和目标物体不同时运动的场合，即眼固定型视觉定位或跟踪问题.

1.2视觉伺服预测控制

有赖于其滚动优化机制,模型预测控制在降低建模难度、抗干扰性和减小控制误差方面具有明显的优势.预测控制与其他的控制方法一个很大的区别也在于控制信号的实施方法上.一般情况下,模型预测控制通过当前采样时刻来解决一个优化问题并且获得当前采样时刻和未来时刻的控制量,但是只有当前这个采样时刻的控制量被施加到控制系统中.在接下来的采样时刻,重复进行以上的优化问题[20].

视觉伺服预测控制系统选择内部模型控制(IMC)结构，如图1所示.

(1)托辊间距合理加大。托辊间距应同时满足两个条件：一是托辊承载能力及使用寿命要求；二是保证胶带的承载和回程段的最大挠度符合规定要求。当运输物料的松散密度小于1 600 kg/m3时，我国目前采用的DTII型带式输送机的托辊间距取1.2 m。通过动态分析计算，本胶带机确定选用承载托辊间距1.5 m，承载托辊数量减少了20%，直接降低了胶带机成本。

图1　控制系统结构图Fig.1　Structure chart of control system

图1中，被控过程包括机械臂和视觉系统.过程输入U为机械臂控制变量，即机械臂关节角度；输出s(k)为机械臂当前的位置信息；参考值s*(k)为期望的位置信息；sm(k)为系统模型预测得到的位置信息；ε(k)为当前位置信息和系统模型预测的位置信息之间所有的模型误差和干扰.由图1可得

ε(k)=s(k)-sm(k)

(3)

sd(k)=s*(k)-ε(k)=s*(k)-(s(k)-sm(k))

(4)

e(k)=sd(k)-sm(k)=s*(k)-s(k)

(5)

从图1中可以看出：使被控制过程输出s(k)跟踪参考值s*(k)，与使系统模型输出sm(k)跟踪期望值 sd(k)是等价的.假设参考值s*(k)已知，通过在有限预测域NP内的模型可以预测下一时刻位置信息sm(k)的变化.

需要考虑的约束[13-15]有以下3种：

1) 机械约束，即机械臂关节的角度限制，关节角度q，qmin≤q≤qmax.

2) 可视性约束，用来确保目标图像一直处在像素平面，可视区V，Vmin≤V≤Vmax.

3) 控制量约束，即执行机构速度的限制，执行机构速度U，Umin≤U≤Umax.

以上约束可以写成非线性方程，即

(6)

并且定义约束域K.

通过优化算法计算得到期望轨迹sd(k)与系统预测模型输出sm(k)偏差的最小值为

(7)

其中Q(i)为一个对角阵.由期望位置与预测模型输出的位置之差定义性能指标方程J,通过J可以在预测时域Np内得到最优控制序列U,但只有控制序列中的第一个U(k)被输入到被控过程中.总结预测控制用于视觉伺服的步骤如图2所示.

图2　控制系统流程图Fig.2　Flow chart of the control system

2实验设计

2.1NAO机器人视觉系统

将基于视觉反馈的机械臂预测控制用于实验室NAO移动机器人的手臂运动控制.NAO机器人[21-22]的视觉系统是由两个位于机器人的面部且处于同一铅垂面上的摄像机组成，如图3所示.NAO机器人眉心位置的摄像机(CameraTop)的位置过高，执行伺服任务过程中容易发生机器人手末端执行器和目标物体没有同时出现在摄像机视野范围内的情况，从而导致不能全面地获取两者之间的相对信息，继而影响到对机器人手的动作控制.另一个位于嘴部位置的摄像机(CameraButton)，位置更为合适，可以较全面地扫描周边环境，实现辅助机器人手的运动控制.由于NAO机器人视觉系统具有以上所述的局限性，而且两个摄像机不能同时开启，所以很难借助NAO机器人视觉系统构造基于双目立体视觉系统.另外，NAO机器人摄像机具有一定的实验角度范围，查NAO摄像头参数表，得知NAO机器人垂直视觉范围为47.64°，水平视觉范围为60.97°，可见性约束是为了确保目标物和机械手在NAO的视野范围内.

图3　NAO视觉系统Fig.3　NAO vision system

如图4所示，Naomark是由AldebaranRobotics公司设计，以一个白色圆圈为圆心，周围有若干圆圈为中心打开的白色扇形，白色圆圈里的数字叫做MarkID，是对不同Naomark的标记.视觉特征的正确选取对于基于图像的视觉伺服系统具有举足轻重的作用,但是对于NAO机器人而言，只要将Naomark放在目标物体上，然后将目标物体放在NAO机器人行动范围内的不同位置，NAO机器人便可以把直接寻找目标物体的任务转换为通过检测Naomark，进而实现目标物体的间接定位.

图4　不同的NaomarksFig.4　Different Naomarks

2.2NAO机器人手臂的雅克比矩阵

NAO机器人手臂具有5个自由度，其左手臂各关节名称如表1所示.通过实验测试发现，其中LWristYaw和LElbowYaw对轨迹规划的影响较小可以忽略不计，因此预先设定为固定的角度.从图像雅克比矩阵的物理定义出发，通过观察已知的机器人运动所引起的特征变化，来估计相应的图像雅克比矩阵.具体操作方法是：根据机器人手臂的自由度n，选取m(m≥n)个充分有效的图像特征，在具体实施每一步控制前，让手臂完成n次试探运动，并观察相应的图像变化，然后用最小二乘法估计得到当前的图像雅克比矩阵.

对于NAO机器人，因为控制对象是NAO机器人手臂，机械约束和控制量约束可以合并为一类，它们的约束范围如表1所示.因LWristYaw和LElbowYaw预先设定为固定值，所以只要考虑LShoulderPitch, LShoulderRoll及 LElbowRoll三个关节的范围.预测控制输出的是下一步三个关节的角度，因此只需把三个关节的范围作为状态变量约束.

表1　NAO机械臂关节参数表

3实验场景及结果

实验场景如图5所示，分别在墙壁和NAO机器人手部贴上Naomark标签，它们在NAO机器人视觉系统中图像如图6所示,在机器人的视野范围内选择不同的位置作为期望位置，利用预测控制求取手臂关节角度，控制NAO手末端向着期望位置靠近.

图5　实验场景Fig.5　Experiment scene

图6　Nao视野场景Fig.6　Experiment scene in NAO vision

实验1为定值控制实验，即期望位置固定在NAO视野范围内，NAO手臂的初始位置是在NAO视野范围内任意给定，共进行了4次实验，结果如表2所示，其中y为循环结束时机械臂末端与期望位置之间的空间距离.从表2中可以看出：机械臂最后停在离期望位置距离0.2 cm左右的位置，控制误差相对来说比较小.实验1的手臂末端控制误差曲线如图7所示，从图7中也可以看出:机械臂末端与期望位置之间的空间距离y不断减少，整个过程动态响应令人满意.

图7　定值控制实验结果图Fig.7　Experimental result of fixed value control

实验坐标初始位置/cm期望位置/cm最后位置/cm循环数/次y/cm1X4.604.494.29Y2.162.132.18Z48.8647.5347.51100.212X4.304.774.63Y2.311.932.18Z47.4846.9646.9550.283X4.304.784.70Y2.312.062.08Z47.4847.0146.8470.194X4.573.733.84Y2.011.872.12Z46.7347.4047.4560.28

实验2为跟踪控制实验，期望位置在实验过程中有发生变化，但一定处于NAO视野范围内.NAO手臂的初始位置是在NAO视野范围内任意给定.期望位置在第5次循环时发生变化，实验结果如图8所示.从图8中可以看出：机械手不断朝着期望位置靠拢，当期望位置发生变化后，机械手又朝着更新后的期望位置靠拢.

作为对比，实验3使用同一组初始位置和期望位置进行了两组实验，一组运用了基于视觉反馈的机械臂预测控制方法，一组运用传统的基于图像的视觉伺服方法，该方法也采用基于图像雅克比矩阵的模型，但是控制器选用比例反馈控制器.两组实验的实验结果如图9所示.从图9中可以看出：比例控制结果曲线在第4次就不再更新，因为传统的基于图像的视觉伺服方法没有考虑约束问题，在第3次控制量传给机器人后，机械臂已经超出了机器人的视野范围，获取不到最新的位置信息，导致伺服任务失败.对比上述实验效果，验证了带有约束的视觉伺服预测控制算法的可行性.

图8　跟踪控制实验结果图Fig.8　Experimental result of tracking control

图9　实验结果Fig.9　Experiment result

4结论

针对机械臂的视觉伺服控制，在明确考虑系统约束问题的前提下，提出简化的基于视觉反馈的机械臂预测控制方法.通过实验，测试了这一方法的可行性.实验表明：以固定的图像雅克比矩阵模型为基础的视觉预测控制方法，实现机械手向目标逼近是可行的，建模简单，算法计算量小容易实现，并且控制误差在一定的允许范围内.而要进一步保证复杂场景下的视觉伺服控制精度，则需考虑作为预测模型的图像雅克比矩阵的在线估计问题.

参考文献：

[1]CHAUMETTE F, HUTCHINSON S. Visual servo control I basic approaches[J]. IEEE robotics & automation magazine,2006,13(4):82-90.

[2]林靖，陈辉堂，王月娟，等．机器人视觉伺服系统的研究[J]．控制理论与应用，2000，17(4):476-481．

[3]薛定宇，项龙江，司秉玉，等．视觉伺服分类及其动态过程[J]．东北大学学报(自然科学版)，2003，24(6):543-547．

[4]方勇纯．机器人视觉伺服研究综述[J]．智能系统学报，2008，3(2):109-114．

[5]贾丙西，刘山，张凯祥，等．机器人视觉伺服研究进展：视觉系统与控制策略[J]．自动化学报，2015，41(5):861-873．

[6]鲍官军,荀一,戚利勇,等.机器视觉在黄瓜采摘机器人中的应用研究[J]．浙江工业大学学报,2010,38(1):114-118.

[7]王婷婷.带有约束的机器人视觉伺服控制方法的研究[D].无锡：江南大学，2012.

[8]CHESI G, HUNG Y S. Global path-planning for constrained

and optimal visual servoing[J]. IEEE transactions on robotics,2007,23(5):1050-1060.

[9]董辉,仲晓帆,黄胜．一种六关节机器人关节空间轨迹规划方法[J]．浙江工业大学学报,2015,43(3):336-339．

[10]李优新,毛宗源,田联房.基于图像矩与神经网络的机器人四自由度视觉伺服[J].控制理论与应用,2009,26(10):1162-1166.

[11]SIRADJUDDIN I, BEHERA L, MCGINNITY T M, et al. Image-based visual servoing of a 7-DOF robot manipulator using an adaptive distributed fuzzy PD controller[J].IEEE/ASME transactions on mechatronics,2014,19(2):512-523.

[12]LóPEZ M B. Fuzzy vs nonfuzzy in 2D visual servoing for robot manipulators[J]. International journal of advanced robotic systems,2013,10(2):1-11.

[13]ALLIBERT G, COURTIAL E, TOURE Y. Real-time visual predictive controller for image-based trajectory tracking of mobile robot[C]//International Fedration of Automatic Control World Congress. Seoul, Korea: International Federation of Automatic Control,2008:11244-11249.

[14]ALLIBERT G, COURTIAL E, CHAUMETTE F．Predictive control for con-strained image-based visual servoing[J]. IEEE transactions on robotics,2010,26(5):933-939.

[15]ALLIBERT G, COURTIAL E, CHAUMETTE F. Visual servoing via nonlinear predictive control[J]. Lecture notes in control & information sciences,2010,401:375-393.

[16]张晓颖，李德伟，席裕庚，等．六自由度机械臂约束预测控制系统的设计[J]．控制理论与应用，2014,31(11):1464-1472.

[17]FERREIRA P, PINTO J R C. Visual based predictive control for a six degrees of freedom robot[C]//Emerging Technologies and Factory Automation. Prague: IEEE,2006:846-853.

[18]LAZAR C, BURTACU A．Visual servoing of robot manipulators using model-based predictive control[C]//Proceedings of IEEE International Conference on Industrial Informatics. Cardiff, Wales: IEEE，2009:690-695.

[19]俞立,唐俊淮,陈军统．一种基于观测范围约束的改进EKF-SLAM 算法[J].浙江工业大学学报,2014,42(5):566-571.

[20]邢满荣，纪春明.浅析预测控制的特点及应用[J].中国科技信息，2006(9)：188-188.

[21]张生.NAO机器人的目标识别与定位研究[D].合肥：安徽大学,2013.

[22]孙漫漫.数据驱动的NAO机器人关节运动控制[D].武汉：武汉理工大学，2014.

(责任编辑：刘岩)

Predictive control of manipulators based on visual feedback

YANG Maying, ZHENG Yafei

(College of Information Engineering, Zhejiang University of Technology, Hangzhou 310023, China)

Abstract:In regarding to the vision-based target capturing of robots, an image-based monocular vision servo system is constructed. The model based on image-based jacobian matrix is obtained in advance. And the predictive control algorithm is introduced into the visual servo control of manipulators under the premise of target visibility constraints and actuator constraints of robots. In order to realize the control of end position of manipulator, a simplified multivariable mechanical arm visual servo controller is designed using motion prediction based on visual features. Then predictive control method based on visual feedback of the robot manipulator is applied to NAO robot, and the rapid identification of object is achieved by using Naomark label with its own. Experimental results demonstrate that the proposed method is feasible. Moreover, the algorithm is simple and easy to be implemented, and the control precision is satisfied.

Keywords:visual servo control; image-based jacobian matrix; model predictive control; NAO robot

收稿日期：2015-11-18

作者简介：杨马英(1966—)，女，浙江海宁人，教授，研究方向为过程控制与机器人控制技术，E-mail：myyang@zjut.edu.cn.

中图分类号：TP273

文献标志码：A

文章编号：1006-4303(2016)03-0260-06