非连续路段下移动机器人的轨迹跟踪

孟 静 ，高晓丁 ，陈 罡

(1．西安工程大学机电工程学院，陕西西安710000;2．浙江纺织服装学院机电学院，浙江宁波315200)

0 引 言

基于视觉的移动机器人轨迹跟踪已成为研究热点，目前对移动机器人的轨迹跟踪多是连续路段。如在自动化生产车间里，机器人沿着固定轨迹完成搬运货物等重复性的工作;在医院里机器人充当传送病历及常用资料的传递员等［1］。文献［2］基于Backstepping 思想设计了一种滑模变结构轨迹跟踪控制系统，很好实现了移动机器人的轨迹跟踪。文献［3］采用Lyapunov 函数直接法提出了一种轨迹跟踪控制算法，分析了系统的渐进稳定性。对于移动机器人在非连续中的运动情况，目前尚未有系统的研究，本研究就轮式移动机器人在非连续路段中如何实现精确的轨迹控制等问题进行讨论。

对于路面状况的分析，目前多是针对于道路裂缝等问题进行研究，本研究在裂缝的识别与算法的基础上，对非连续路段问题进行深入分析与研究，通过合理的算法实现对移动机器人在非连续路段中的稳定控制。

1 运动学模型及目标信息的获取

1．1 运动学模型

笔者研究的轮式移动机器人采用两个驱动轮、一个万向轮的结构。左、右两个驱动轮安装在同一轴线上，分别安装两个电机驱动机器人运动，当两个电机的转动速度不同时，左、右两轮将会产生差动，则可实现移动机器人的转弯动作［4］。由于万向轮仅起支撑作用，故忽略其在运动学模型中的作用，建立的移动机器人的运动模型如图1 所示。

图1 运动学模型

机器人的位姿可由它中心所在点的坐标及方向角表示，令:

Pc=(xc，yc，θc)T，q=(v，w)T

式中:(xc，yc)T—移动机器人的位置;θc—移动机器人前进方向与x 轴的夹角;v，w—移动机器人的线速度和角速度。

它们在运动学模型中是控制输入，移动机器人的运动学方程为:

本研究中的轨迹跟踪是让机器人沿着给定路径行走，当机器人从当前位姿Pc=(xc，yc，θc)T移动到目标位姿Pr=(xr，yr，θr)T时，产生了一个新的坐标系xe－ye，令移动机器人的位姿误差为Pe=(xe，ye，θe)T，其中，θe=θr－θc。设新坐标系xe－ye与原坐标系x－y之间的夹角即为θc，可以得到机器人位姿误差方程:

位姿误差微分方程为:

由以上分析可知，移动机器人运动学模型的轨迹跟踪可以定义为寻找有界输入v 和w，使对任意初始误差，位姿误差微分方程在该控制输入作用下有界且=0。

1．2 摄像机的标定

摄像机的标定方法一般是在摄像机前放置一个立体标靶，用上面位置已知的点作为特征点，采集标靶图像后，利用这些点的图像坐标和三维空间坐标建立方程，解方程即可获得摄像机的内、外参数［5］。本研究采用Faugeras 的线性模型摄像机标定法对摄像机进行标定。设点的世界坐标系已知，摄像机的内参数模型为:

式中:Min—摄像机的内参数矩阵;(xc，yc，zc)—景物点在摄像机坐标系下的坐标;(u，v)—景物点的图像坐标。

摄像机的外参数模型为:

式中:(xw，yw，zw)—景物点在世界坐标系下的坐标且已知;cMw—外参数矩阵。

由式(4，5)可得:

将式(6)展开并消掉zc后，得下式:

由于空间的每个点都符合式(7)两个方程，于是可得式(8)所示矩阵:

将外参数矩阵cMw和M 矩阵写成式(9)形式，可得式(10)所示矩阵:

cMw中的R 是单位正交矩阵。利用单位正交矩阵的性质，可以从M 矩阵中分解出摄像机的内外参数，如下式:

求出以上参数即可完成对摄像机的标定。

1．3 轨迹信息的获取

本研究采用双目立体视觉系统确定空间目标点相对于摄像机的三维空间位姿。设两台摄像机C1和C2的内参数采用四参数模型，相对外参数用c1Mc2表示。空间点P 在摄像机C1中的图像坐标为(u1，v1)T，则像点坐标P1c1表示为:

空间点P 在摄像机C1光轴中心点与点P1c1构成的直线上，即符合方程:

将点P1c2在C2中的坐标转换为在C1中的坐标，如下式:

同理可求得点P 在C2中的像点坐标以及方程

式(16)中Px、Py和Pz构成c1Mc2的位置偏移量。直线方程(13，16)的交点即为空间点P。至此可以求出空间点的坐标，同理可以通过判定其他点的位置获取目标轨迹［6-7］。

2 道路模型

2．1 连续路段

当为连续路段时，本研究选取直线模型y =kx +b作为轨迹模型，然后利用路径线上的n 个像素点(假设像素点坐标为(ai，ci)其中n ＞1，1≤i≤n)和最小二乘法对路径线模型的参数k 和b 进行拟合计算［8］:

最后，根据计算出的直线模型即可确定移动机器人的期望跟踪轨迹。

2．2 非连续路段

当移动机器人在轨迹跟踪过程中出现如图2 所示的非连续路段时，需先将摄像头采集到的图像进行图像预处理，突出图像中的非连续路段，再运用二值化的方法进行图像区域分割，提取出图像中非连续路段信息［9-10］。

二值化方法的关键在于阈值的选取与设定，本研究主要采用大津法进行阈值的确定，该方法的原理如下:

设图2 中图像的灰度值集合为P={0，1，2，3，…，255}，灰度值为j 的像素个数为mj，则整幅图的像素总数为:

图2 非连续路段

则对应灰度值为j 的像素概率分布为:

式中:j∈P，Qj≥0，Qj=1。

则整幅图像的灰度平均值μ，方差σ2可表示为:

假若整幅图像以灰度值作为阈值，那么将图像分割成为P1={0，1，…T}，P2={T +1，T +2，…255}两个类Z1，Z2，此时，类Z1和类Z2发生的概率P1，P2，可以表述为:

并且有P1+P2=1。

从而这两类的均值分别为:

这两类的方差分别为:

这样就可以得到图像的类间方差为:

图3 非连续路段的二值化图像

在检测到非连续路段后，还需计算非连续路段的宽度，看其是否超出机器人行走的安全范围，计算方法为［11］:

(1)求出二值非连续路段图像水平投影Y。

(2)设在坐标点j 处的投影值为pro［j］，pro［j］大于0。

(3)遍历二值非连续路段细化图像，选取j(j，y)点及其相邻点i(i，x)，计算此处裂缝的倾角α。

(4)计算裂缝宽度值W［j］。

(5)对各个投影点为横坐标的非连续路段点处的宽度都进行上述计算，最大值即为非连续路段点的最大宽度。

当所计算出的非连续路段点的宽度在机器人行走的安全范围内时，机器人按照所给的轨迹继续追踪，当超出机器人行走的安全范围时，机器人将停止或后退。

3 轨迹跟踪

3．1 轨迹跟踪控制原理

在机器人的运动过程中，可通过机器人的几何中心坐标，生成实际运动轨迹曲线，并比较实际运动轨迹与理论运动轨迹的差值。当二者误差小于θα时，不需要对机器人的运动进行纠正;当二者的误差超出θα时，则可根据设定的算法程序，控制机器人左、右电机以不同的速度转动，完成对机器人运动位姿的纠正与误差补偿，从而实现对机器人运动的闭环控制与自动跟踪。经过反复控制器调节，最终使得系统位姿误差Pe趋于零。移动机器人的轨迹跟踪框图如图4 所示。

3．2 控制器的设计

为了使移动机器人的跟踪误差快速收敛于零，获得良好的轨迹跟踪效果，本研究设计了基于Lyapunov函数的控制器［12-14］，其控制律如下:

图4 轨迹跟踪控制框图

令控制参数a1，a2，aθ有界，且都大于零。

构造Lyapunov 函数为:

结合式(2，3)对式(34)求导得:

将式(33)代入式(35)得:

根据Lyapunov 稳定性判据可知，当a1，a2，aθ有界且都大于零时，用式(33)表示的控制率能使轮式移动机器人位姿误差趋近于零。

4 仿真试验

为了验证控制算法的有效性，本研究用Matlab 进行算法编程，对机器人直线轨迹跟踪的连续路段和非连续路段进行仿真［15-16］。算法程序流程图如图5 所示。参考轨迹的初始位置为xr(0)=0，yr(0)=0，θr(0)=π/4 控制输入为v=0．2 m/s，w=0，角度误差θα=7°，移动机器人的初始位置为xc(0)=1．5，yc(0)=1．5，θc(0)=2π/3;控制参数a1=a2=10，aθ=50，裂缝宽度的安全值为W［j］=5 mm，SV=2;直线轨迹跟踪效果图如图6 所示;当为连续路段时，仿真结果如图7 所示。当t=3．2 s 出现非连续路段时，非连续路段宽度安全范围如图8 所示，非安全范围如图9 所示。

图5 算法程序流程图

图6 直线轨迹跟踪效果图

图7 连续路段轨迹跟踪效果图

图8 非连续可走路段轨迹跟踪效果图

移动机器人的跟踪结果与偏差变化如图6 所示。机器人在连续路段中的轨迹跟踪情况如图7 所示;图8 中，机器人在t=3．1 s 时遇到非连续路段，其宽度小于5 mm，故机器人可以行走，但在此刻位移误差出现了波动，在t=10 s 时实现了稳定跟踪，收敛到理想轨迹;图9 中在t=3．2 s 时遇到非连续路段，其宽度大于5 mm，机器人不可以行走，故最终停留原地;图10 中在t=2．5 s 时位姿误差收敛到零，从而验证了所设计的控制率能够实现移动机器人对给定轨迹的跟踪。

图9 非连续不可走路段轨迹跟踪效果图

图10 位姿误差变化曲线

5 结束语

本研究讨论了移动机器人在非连续路段下的轨迹跟踪问题，利用二值化方法对非连续路段图像进行处理，通过大津法对阈值进行确定来提取图像的非连续路段信息，并设计了基于Lyapunov 函数的控制器实现了机器人对给定轨迹的跟踪，最后用Matlab 进行仿真，仿真结果证明了该控制率及算法的可行性。通过对非连续路段的处理，使机器人能够更好地实现对道路轨迹的跟踪，防止机器人在运行过程中因路面的破损而损坏机器人。

与其他算法进行比较，大津法能够选取出理想的阈值，但在很多情况下不是最佳的分割，在后续内容中，本研究将着重于最佳阈值的分割进行研究。

［1］日本机器人学会．机器人技术手册(新版)［M］．宗光华，称君实译．北京:科学出版社．2008．

［2］GREYY W，PONDAR． The URANUS mobile robot［R］．The Robotics Institute Technical Report． Carnegie-Mellon University，1986:127-129．

［3］胡浚涛，李亚荣，李 林．轮式移动机器人轨迹跟踪控制研究［J］．仪表技术，2013(4):27-28．

［4］上官望义．轮式移动机器人移动性能研究及样机设计开发［D］．西安:西安理工大学机仪学院，2009．

［5］王宝磊，王朝立，李 菲，等．基于双目视觉伺服反馈的非完整移动机器人轨迹跟踪［J］． 测控自动化，2012，28(10):125．

［6］张显伟，吴忠伟，张凤海，等．移动机器人模糊滑模轨迹跟踪控制［J］．长春工业大学，2013，34(4):374-375．

［7］曹 洋，项龙江，徐心和．基于全局视觉的轮式移动机器人轨迹跟踪控制［J］．机器人，2004，26(1):79-82．

［8］李晋蕙． 用图像处理的方法检测公路路面裂缝类病害［J］．长安大学学报:自然科学版，2004，24(3):25-29．

［9］肖旺新，张 雪，黄 卫，等．路面破损自动识别的一种新算法［J］．公路交通科技，2005，22(11):75-78．

［10］孙波成，邱延峻． 基于图像处理的路面裂缝识别研究［J］．重庆交通大学学报:自然科学版，2008，27(1):62-64．

［11］肖 靖．道路裂缝识别算法研究［D］．北京:北京邮电大学理学院，2009．

［12］刘 磊，向 平，王永骥，等．非完整约束下的轮式移动机器人轨迹跟踪［J］．清华大学学报:自然科学版，2007，47(S2):1885-1889．

［13］岳龙旺，朱敬花．基于轮式移动平台的新型湿式清洁机器人系统［J］．轻工机械，2013，31(6):66-68．

［14］罗 剑，雷 勇．智能移动机器人中视觉跟踪算法研究［J］．仪器仪表学报，2009，30(6):336-340．

［15］晏祖根，李 明，徐克非，等．高速机器人分拣系统机器视觉技术的研究［J］．包装与食品机械，2014(1):28-31．

［16］刘金琨．机器人控制系统的设计与Matlab 仿真［M］．北京:清华大学出版社，2008．