ＣｏｍｐｕｔｅｒＥｎｇｉｎｅｅｒｉｎｇａｎｄＡｐｐｌｉｃａｔｉｏｎｓ计算机工程与应用２００８，４４（１８）
１引言
随着人工智能的快速发展和人们生活质量的日益提高，越来越多的不同种类的智能机器人开始进入家庭服务业，各种智能型家庭服务机器人的研究成为许多学者关注的焦点。室内环境下对规划好的轨迹进行跟踪，是移动服务机器人导航研究中的重要内容之一。基于视觉伺服的控制方式因具有信息量大、效率高等优点而越来越多地被用于移动机器人的控制。文献［１，２］采用基于针孔摄像机实现了非完整约束移动机器人的视觉伺服控制。文献［３］采用视觉方式，实现机器人的轨迹跟踪控制。
近年来，以Ｋｏｋｏｔｏｖｉｃ及其合作者发展起来的ｂａｃｋｓｔｅｐｐｉｎｇ法引起了有关学者的高度重视。这种方法把不确定参数的自适应调节函数和一个已知Ｌｙａｐｕｎｏｖ函数的虚拟控制系统的镇定函数等联系起来，然后通过积分器后推，逐步修正算法，设计镇定控制器，获得系统最终真正的控制规律，实现系统的全局调节或跟踪［６］。
本文将ｂａｃｋｓｔｅｐｐｉｎｇ方法应用于移动服务机器人的视觉伺服中，实现了轮式移动服务机器人视觉空间内的轨迹跟踪控制。２移动服务机器人的运动学模型
２．１任务空间内的服务机器人运动学模型
本文利用较为典型的双轮驱动移动机器人平台作为研究
对象，其结构如图１所示。取两个驱动轮的轴中心点Ｍ（ｘ
Ｍ
，ｙ
Ｍ
）为服务机器人的位置，取图中!为其方向角，即前进方向与ｘ
轴的夹角。令ｐ＝（ｘ
Ｍ
ｙＭ!）Ｔ为服务机器人任务空间内的位姿，ｖ和"分别表示服务机器人任务空间内的线速度和角速度，令ｑ＝（ｖ"）Ｔ，则
服务机器人任务空间内的运动学方程为
ｐ!＝
ｘ!Ｍ
ｙ!Ｍ
!!
!
"
"
"
"
"
"
"
"
"
"
"
#
$
%
%
%
%
%
%
%
%
%
%
%
&
＝
ｃｏｓ!０
ｓｉｎ!０
０
!
"
"
"
"
"
"
""
#
$
%
%
%
%
%
%
%%
&
１
ｑ（１）２．２视觉空间内的服务机器人运动学模型
令ｐ’＝（ｘ(Ｍｙ(Ｍ!’）Ｔ为服务机器人视觉空间内的位姿；ｖ(和")分别为机器人在视觉空间内的线速度和角速度，令ｑ’＝（ｖ(")）Ｔ，则服务机器人视觉空间内的运动学方程为
基于视觉的服务机器人轨迹跟踪控制
张立勋１，卜迟武１，２
ＺＨＡＮＧＬｉ－ｘｕｎ１，ＢＵＣｈｉ－ｗｕ１，２
１．哈尔滨工程大学机电工程学院，哈尔滨１５０００１
２．哈尔滨商业大学轻工学院，哈尔滨１５００２８
１．ＳｃｈｏｏｌｏｆＭｅｃｈａｎｒｏｎｉｃ，ＨａｒｂｉｎＥｎｇｉｎｅｅｒｉｎｇＵｎｉｖｅｒｓｉｔｙ，Ｈａｒｂｉｎ１５０００１，Ｃｈｉｎａ
２．ＳｃｈｏｏｌｏｆＬｉｇｈｔＩｎｄｕｓｔｒｙ，ＨａｒｂｉｎＵｎｉｖｅｒｓｉｔｙｏｆＣｏｍｍｅｒｃｅ，Ｈａｒｂｉｎ１５００２８，Ｃｈｉｎａ
Ｅ－ｍａｉｌ：ｂｕｃｈｉｗｕ＠１２６．ｃｏｍ
ＺＨＡＮＧＬｉ－ｘｕｎ，ＢＵＣｈｉ－ｗｕ．Ｔｒａｊｅｃｔｏｒｙｔｒａｃｋｉｎｇｃｏｎｔｒｏｌｏｆｓｅｒｖｉｃｅｒｏｂｏｔｂａｓｅｄｏｎｖｉｓｉｏｎ．ＣｏｍｐｕｔｅｒＥｎｇｉｎｅｅｒｉｎｇａｎｄＡｐｐｌｉｃａｔｉｏｎｓ，２００８，４４（１８）：１９４－１９６．
Ａｂｓｔｒａｃｔ：Ｖｅｌｏｃｉｔｙｖｅｃｔｏｒｔｒａｎｓｆｏｒｍａｔｉｏｎｆｒｏｍｖｉｓｉｏｎｓｐａｃｅｔｏｔａｓｋｓｐａｃｅｉｓｆｉｎｉｓｈｅｄ，ｂａｓｅｄｏｎｔｈｅｋｉｎｅｍａ
ｔｉｃｍｏｄｅｌｏｆｍｏｂｉｌｅｓｅｒ－ｖｉｃｅｒｏｂｏｔ，ａｎｄｉｍａｇｉｎｇｐｒｉｎｃｉｐｌｅｏｆｐｉｎｈｏｌｅｃａｍｅｒａ．Ａｇｌｏｂａｌａｓｙｍｐｔｏｔｉｃａｌｌｙｓｔａｂｌｅｔｒａｃｋｉｎｇｃｏｎｔｒｏｌｌｅｒｉｓｄｅｓｉｇｎｅｄｂｙｕｓｉｎｇｖｉｓｕａｌｓｅｒｖｉｎｇａｎｄｂａｃｋｓｔｅｐｐｉｎｇｍｅｔｈｏｄ，ｍｅａｎｗｈｉｌｅ，ｔｈｅＬｙａｐｕｎｏｖｔｈｅｏｒｙｉｓｕｓｅｄｔｏｖｅｒｉｆｙｔｈｅｓｔａｂｉｌｉｔｙｏｆｔｈｅｃｏｎｔｒｏｌｌｅｒ．Ｓｉｍｕｌａｔｉｏｎｒｅｓｕｌｔｓｓｈｏｗｔｈｅｅｆｆｅｃｔｉｖｅｎｅｓｓａｎｄｃｏｒｒｅｃｔｎｅｓｓｏｆｔｈｅｄｅｓｉｇｎｅｄｃｏｎｔｒｏｌｌｅｒ．
Ｋｅｙｗｏｒｄｓ：ｓｅｒｖｉｃｅｒｏｂｏｔ；ｖｉｓｕａｌｓｅｒｖｏｉｎｇ；ｂａｃｋｓｔｅｐｐｉｎｇ；ｔｒａｊｅｃｔｏｒｙｔｒａｃｋｉｎｇ
摘要：根据移动服务机器人的运动学模型和针孔摄像机成像原理，完成了机器人的速度向量由视觉空间到任务空间的变换。采用视觉伺服控制方式，结合反步设计思想，设计了具有全局渐近稳定的轨迹跟踪控制器，并利用Ｌｙａｐｕｎｏｖ函数进行稳定性分析。仿真结果验证了所设计控制器的有效性和正确性。
关键词：服务机器人；视觉伺服；反步；轨迹跟踪
ＤＯＩ：１０．３７７８／ｊ．ｉｓｓｎ．１００２－８３３１．２００８．１８．０６１文章编号：
１００２－８３３１（２００８）１８－０１９４－０３文献标识码：Ａ中图分类号：ＴＰ２４２
基金项目：国家自然科学基金（ｔｈｅＮａｔｉｏｎａｌＮａｔｕｒａｌＳｃｉｅｎｃｅＦｏｕｎｄａｔｉｏｎｏｆＣｈｉｎａｕｎｄｅｒＧｒａｎｔＮｏ．６０５７５０５３）。
作者简介：张立勋（１９６２－），男，博士，教授，博士生导师，主要研究方向：机器人。
收稿日期：２００７－１２－２１修回日期：２００８－０２－２５
１９４
２００８，４４（１８）ｐ!・
＝ｘ
"・
Ｍｙ
"・Ｍ!
!・#$$$$$$$$$$$$$%
&
’’’’’’’’’’’’’(
＝
ｃｏｓ!!０ｓｉｎ!!００
#)))))))))%
&
’’’’’’’’’(
１
ｑ
!（２）
２．３任务空间到视觉空间的映射
摄像机针孔模型［４］如图２所示。设任务空间内任一目标点Ｐ在世界坐标系｛Ｏｗ，ｘｗ，ｙｗ，ｚｗ｝下的坐标为（ｘｗ，ｙｗ，ｚｗ），Ｐ在摄像机
坐标系｛Ｏｃ，ｘｃ，ｙｃ，ｚｃ｝下的坐标为（ｘｃ，ｙｃ，ｚｃ），其中Ｏｃ为摄像机的光心，且ｚｃ轴与光轴重合。ｐ（ｘ，ｙ）是理想针孔摄像机模型下点Ｐ在图像平面上所成像点的坐标，ｆ是摄像机的焦距。
根据摄像机的成像原理得
ｘ
*+ｙ＝ｆｚｃ
ＨＲ（!０
）ｘｃ
ｙｃ*+（３）其中，Ｈ＝!ｘ０
０!*+ｙ
，
!ｘ和!ｙ表示每个像素在ｘ轴和ｙ轴方向上的物理尺寸；Ｒ（!０
）＝ｃｏｓ!０
－ｓｉｎ!
０
ｓｉｎ!０
ｃｏｓ!
０
*+为旋转矩阵，表示因摄像机成像平面坐标轴相互不正交引出的倾斜因子。
对服务机器人中心点Ｍ有
ｘ
"Ｍｙ"Ｍ
*+
＝
ｆｚｃＨＲ
（!０）ｘＭｙＭ
*+
（４）
对式（４）两边求导，并把式（１）、式（２）代入得ｖ
"ｃｏｓ!!ｖ"ｓｉｎ!
!*+
＝
ｆｚｃｖ!ｘｃｏｓ
（!＋!０）ｖ!ｙｓｉｎ（!＋!０*+
）（５）
对式（５）变形处理可得ｖ＝
ｚｃ
ｆ［!ｘｃｏｓ
（!＋!０）ｃｏｓ（!!）＋!ｙｓｉｎ（!＋!０）ｓｉｎ（!!）］（６）
由式（５）知ｔａｎ!!
＝!ｘ!ｙｔａｎ（!＋!０）
（７）
对式（７）两边求导得"＝
!ｙ!ｘｃｏｓ２
（!＋!０）ｃｏｓ２!!",（８）至此，完成了服务机器人从视觉空间到任务空间的速度向
量的变换。
３视觉空间的轨迹跟踪控制原理３．１基于视觉的运动控制系统组成
基于视觉的移动服务机器人轨迹跟踪控制原理如图３所示。控制系统的输入为参考位姿ｐｒ的图像特征和期望速度ｑｒ，
输出为机器人当前位姿ｐ!。采用双闭环结构，内环为机器人运动
伺服控制，外环采用视觉伺服控制。
３．２误差微分方程的建立
图像平面内的轨迹跟踪可通过对具有位姿ｐｒ＝（ｘｒｙｒ!ｒ）
Ｔ
和速度ｑｒ＝（ｖｒ"ｒ）Ｔ
的参考小车的跟踪来实现。令服务机器人的位置跟踪误差为ｐｅ＝
（ｘｅｙｅ!ｅ）Ｔ
，则服务机器人视觉空间内的位姿误差方程为［５］
ｐｅ＝ｘｅ
ｙｅ!ｅ
-)
))))))%
&’’’’’’’(
＝
ｃｏｓ!!ｓｉｎ!!
０
－ｓｉｎ!!ｃｏｓ!!
００
０
-)))))))))%
&’’’’’’’’’(１
ｘｒ－ｘ"ｙｒ－ｙ"!ｒ－!!
-)
)))))))))%
&
’’
’
’
’’’
’’’(
（９）
对式（９）求导，并将式（２）代入可得服务机器人位姿误差微分方程为
ｐ!ｅ＝ｘ
!ｅｙ!ｅ!
!ｅ
-))))))))))%
&’’’’’’’’’’(＝－ｖ
"＋ｙｅ",＋ｖｒｃｏｓ!ｅ－ｘｅ"
,＋ｖｒｓｉｎ!ｅ－"
,
＋"ｒ-))))))))))%
&’’’’’’’’’’(
（１０）
服务机器人运动学模型的轨迹跟踪是要寻有界输入ｑ
!＝（ｖ"",）Ｔ
，对任意的初始误差，机器人位姿误差微分方程（１０）在
该控制输入作用下ｐｅ＝（ｘｅｙｅ!ｅ）Ｔ
，有界，且ｌｉｍｔ→∞
‖ｘｅｙｅ!ｅ‖Ｔ＝０。
４移动服务机器人的轨迹跟踪控制
对移动服务机器人的位姿误差微分方程（１０），当ｘｅ＝０时，对部分Ｌｙａｐｕｎｏｖ函数Ｖｙ＝１２
ｙｅ２
考察。当!ｅ＝－ａｒｃｔａｎ（ｖｒｙｅ）可使ｙｅ
收敛，即只要ｘｅ收敛到０，且!ｅ收敛到－ａｒｃｔａｎ（ｖｒｙｅ），则ｙｅ就可
收敛到０，从而最终!ｅ收敛到０。所以，只要设计合适的ｑ＝（ｖ"）Ｔ，使ｘｅ趋于０，!ｅ趋于－ａｒｃｔａｎ
（ｖｒｙｅ），就可以使系统最终收敛。取Ｌｙａｐｕｎｏｖ函数为［６］
Ｖ＝
１２ｘｅ２＋１２ｙｅ２＋２（
１－ｃｏｓ!!ｅ２
）（１１）
其中，!!ｅ＝!ｅ＋ａｒｃｔａｎ
（ｖｒｙｅ）。对式（１１）关于时间求导，并结合式（１０）可得
Ｖ!＝ｘｅ（ｙｅ"－ｖ＋ｖｒｃｏｓ!ｅ）＋ｙｅ（－ｘｅ"＋ｖｒｓｉｎ!ｅ
）＋ｓｉｎ!!ｅ２［"ｒ－"－0#0ｖｒｖ!ｒ－0#0ｙｅ（－ｘｅ"＋ｖｒｓｉｎ!ｅ）］＝ｘｅ（－ｖ"＋ｖｒｃｏｓ!ｅ
＋0#0ｙｅ"ｓｉｎ!!ｅ２）＋ｙｅｖｒｓｉｎ#＋ｙｅｖｒ［ｓｉｎ（!!ｅ＋#）－ｓｉｎ#］＋ｓｉｎ!!ｅ２［"ｒ－"－
0#0ｖｒｖ!ｒ－0#0ｙｅｖｒｓｉｎ!ｅ］＝ｘｅ（－ｖ＋ｖｒｃｏｓ!ｅ
＋0#0ｙｅ"ｓｉｎ!!ｅ２）＋ｙｅｖｒｓｉｎ#＋ｓｉｎ
!!ｅ２［"ｒ－"－0#0ｖｒｖ!ｒ－0#0ｙｅｖｒｓｉｎ!ｅ＋２ｙｅｖｒｃｏｓ（#＋!!
ｅ
２
）］（１２）
张立勋，卜迟武：基于视觉的服务机器人轨迹跟踪控制
１９５
ＣｏｍｐｕｔｅｒＥｎｇｉｎｅｅｒｉｎｇａｎｄＡｐｐｌｉｃａｔｉｏｎｓ计算机工程与应用
２００８，４４（１８）由式（１２）得ｑ＝
ｖ!"!
＝ｖｒｃｏｓ"ｅ＋
###ｙｅ!ｓｉｎ"
$ｅ２
＋ｃ１ｘｅ!ｒ－###ｖｒｖ!ｒ－###ｙｅｖｒｓｉｎ"ｅ＋２ｙｅｖｒｃｏｓ（#＋"$ｅ２）＋ｃ２ｓｉｎ"$ｅ
２%
&&&&&&&&&&&’
(
)))))))))))*
（１３）
其中，#$#ｖｒ＝ｙｅ１＋
（ｖｒｙｅ）２，#$#ｙｅ＝ｖｒ１＋（ｖｒｙｅ）２，ｃ１、ｃ２为正常数，从而得到控制规律ｑ＝（ｖ!）Ｔ
。
将式（１３）带入式（１２）整理得
Ｖ!＝－ｃ１ｘｅ２－ｙｅｖｒ
ｓｉｎ［ａｒｃｔａｎ（ｙｅｖｒ）］－ｃ２ｓｉｎ２"$ｅ２
≤０（１４）所以，在ｑ的作用下，系统可以实现稳定，且对任意的初始
误差，均能实现ｐｅ＝（ｘｅｙｅ"ｅ）Ｔ
收敛到０，即实现了服务机器人
图像平面内的特征点能够跟踪预定的轨迹特征。
５仿真研究
为了验证服务机器人在已设计的控制规律ｑ作用下轨迹
跟踪效果和误差收敛的速度，分别针对具有不同初始误差的直线和８字形参考轨迹进行仿真分析。
取ｃ１＝ｃ２＝８。设直线参考轨迹ｐｒ＝（ｔｔｔ）Ｔ
，取任意初始误差
ｐｅ＝（－０．５０．５π／６）Ｔ；设８字形参考轨迹ｐｒ＝（ｓｉｎ（ｔ／３）ｓｉｎ（ｔ／６）ｔ）
Ｔ
，取任意初始误差ｐｅ＝（１０．５π／６）Ｔ
。在图３所示的控制方式作用下，服务机器人的轨迹跟踪及跟踪过程中误差变化的ｓｉｍｕｌｉｎｋ仿真结果如图４、图５所示。
由以上仿真结果可以看出，对具有任意初始误差的直线和
８字形参考轨迹，
服务机器人均能对其做出快速有效的跟踪逼近；且位姿误差ｐｅ的组成元素ｘｅ、ｙｅ和"ｅ均能快速趋
于０，
即达到了ｐｅ＝（ｘｅｙｅ"ｅ）Ｔ
有界，且ｌｉｍｔ→∞
‖ｘｅｙｅ"ｅ‖Ｔ＝０。所以，在所设计
的控制方式的作用下，可以实现服务机器人轨迹跟踪控制系统
的快速收敛。
６结论
在分析移动服务机器人运动学模型和针孔摄像机成像原理的基础上，实现了基于视觉伺服的移动服务机器人在运动过程中速度向量的变换，进而构建了服务机器人轨迹跟踪控制结构。引入反步设计方法，并利用Ｌｙａｐｕｎｏｖ方法证明了系统的全局稳定性。通过对具有任意初始误差的直线形和８字形给定轨迹的跟踪，验证了所设计控制器的有效性和正确性。此外，文中所构建的基于视觉伺服的移动服务机器人轨迹跟踪控制结构，可用于其他移动机器人的导航研究。
参考文献：
［１］ＭａｒｉｏｔｔｉｎｉＧＬ，ＯｒｉｏｌｏＧ，ＰｒａｔｔｉｃｈｉｚｚｏＤ．Ｅｐｉｐｏｌｅ－ｂａｓｅｄｖｉｓｕａｌｓｅｒｖｏ－
ｉｎｇｆｏｒｎｏｎｈｏｌｏｎｏｍｉｃｍｏｂｉｌｅｒｏｂｏｔｓ［Ｃ］／／ＰｒｏｃＩＥＥＥＩｎｔＣｏｎｆＲｏｂｏｔ，Ａｕｔｏｍ，２００４：４９７－５０３．
［２］ＰｉａｚｚｉＪ，
ＰｒａｔｔｉｃｈｉｚｚｏＤ，ＶｉｃｉｎｏＡ．Ｖｉｓｕａｌｓｅｒｖｏｉｎｇａｌｏｎｇｅｐｉｐｏｌｅｓ［Ｍ］／／ＢｉｃｃｈｉＡ．ＣｏｎｔｒｏｌＰｒｏｂｌｅｍｓｉｎＲｏｂｏｔｉｃｓ．Ｅｔａｌ，ＮｅｗＹｏｒｋ：Ｓｐｒｉｎｇｅｒ，２００３，４：２１５－２３２．
［３］ＰａｐａｎｉｋｏｌｏｐｏｕｌｏｓＮＰ．Ｖｉｓｕａｌｔｒａｃｋｉｎｇｏｆｍｏｖｉｎｇｔａｒｇｅｔｂｙａｃａｍ－
ｅｒａｍｏｕｎｔｅｄｏｎａｒｏｂｏｔ：
ａｃｏｍｂｉｎａｔｉｏｎｏｆｖｉｓｉｏｎａｎｄｃｏｎｔｒｏｌ［Ｊ］．ＩＥＥＥＴｒａｎｓＲｏｂｏｔＡｕｔｏｍａｔ，１９９３，９（１）：１４－３５．
［４］ＭａｒｉｏｔｔｉｎｉＧＬ，buchi
ＯｒｉｏｌｏＧ，ＰｒａｔｔｉｃｈｉｚｚｏＤ．Ｉｍａｇｅ－ｂａｓｅｄｖｉｓｕａｌｓｅｒｖｏ－ｉｎｇｆｏｒｎｏｎｈｏｌｏｎｏｍｉｃｍｏｂｉｌｅｒｏｂｏｔｓｕｓｉｎｇｅｐｉｐｏｌａｒｇｅｏｍｅｔｒｙ［Ｊ］．ＩＥＥＥＴｒａｎｓａｃｔｉｏｎｓｏｎＲｏｂｏｔｉｃｓ，２００７，２３（１）：８７－９９．
［５］叶涛，
侯增广，谭民，等．移动机器人的滑模轨迹跟踪控制［Ｊ］．高技术通讯，２００４（１１
）：７１－７４．
［６］胡跃明．变结构控制理论与应用［Ｍ］．北京：科学出版社，２００３：２２－２６．
１９６