轮式移动机器人轨迹跟踪与避障研究

张立阳，陈奕梅

（天津工业大学 电气工程与自动化学院，天津 300387）

轮式移动机器人轨迹跟踪与避障研究

张立阳，陈奕梅

（天津工业大学 电气工程与自动化学院，天津 300387）

针对非完整轮式移动机器人轨迹跟踪与避障问题，在跟踪误差微分方程的基础上提出了轨迹跟踪控制律，运用Lyapunov判定其稳定性；与模糊控制方法结合，将传统的人工势场法进行改进，实时改变斥力系数和引力系数，并引出虚拟目标点；插值相邻跟踪点，与轨迹跟踪控制律结合，充分发挥轨迹跟踪和人工势场法的优点，实现同时轨迹跟踪与避障任务。仿真和试验结果显示，该机器人能够有效跟踪轨迹并能避开障碍物，表明该控制方法的有效性。

轮式移动机器人；轨迹跟踪；人工势场法；虚拟目标点

轮式移动机器人WMR是典型的非完整动力学系统，具有广泛的实际应用背景，其轨迹跟踪控制因此得到广泛的研究。大体而言，控制的方法主要有模糊方法[1-2]、神经网络[3-4]、滑模方法[5]、自适应[6]以及极坐标表示下的控制律[7]等。然而，人们不仅希望机器人完成轨迹跟踪任务，还想与避障算法结合，同时完成轨迹跟踪和避障任务。

人工势场法是由Khatib[8]提出的一种虚拟力法，该算法因其原理简单、结构简洁、生成路径平滑等特点，在机器人避障路径规划中有着广泛的应用，非常适合于对路径生成实时性和安全性要求较高的规划任务。该方法得到的规划路径虽然不一定是最短的，但却是最平滑和最安全的。

在此，基于机器人的轨迹跟踪误差微分方程设计出轨迹跟踪控制律，运用李雅谱诺夫第二法判定其稳定性，对传统的人工势场法进行改进，利用模糊系统计算引力系数和斥力系数，进而计算合力并引出虚拟目标点。采用插值法计算相邻跟踪点并应用于轨迹跟踪控制律，最后仿真并试验验证了该方法的有效性。

1 机器人的运动学模型

机器人及参考轨迹如图1所示，轮式移动机器人在坐标系为X-Y的空间平面中，θ为机器人方向角；v为机器人的线速度；ω为机器人的角速度。机器人的位姿可用q=[x，y，θ ]T表示。轮式移动机器人的运动学模型为

图1 机器人及参考轨迹Fig.1 Robot and reference trajectory

5 个声呐位于机器人前部，夹角互为 30°。 Pr（xr，yr）为参考轨迹上的点，以 qr=[xr，yr，θr]T作为机器人的期望位姿，以 ur=[vr，ωr]T作为机器人的期望速度控制。

同样，机器人在期望位姿符合的运行学模型为

根据坐标变换公式，可得局部坐标系Xe-Ye下，用直角坐标描述的非完整性移动机器人跟踪误差qe为

进一步可得机器人的轨迹跟踪误差微分方程[9]为

2 轨迹跟踪及避障算法设计

2.1 轨迹跟踪算法设计

针对非完整轮式移动机器人运动学模型的轨迹跟踪控制问题，可以描述为对于式（4）所描述的系统，在任意初始误差的条件下，寻找控制输入u=，使有界，，并且对此，轨迹跟踪控制律设计如下：

式中：kx>0，ky>0，kθ>0。

将式（5）代入式（4），可得轨迹跟踪控制系统的状态空间方程

式中：xe∈R，ye∈R，θe∈（-π，π］。 选取李雅普诺夫函数为

对式（7）求导，并代入式（6），化简后得

显然 V（qe）≥0，当且仅当 qe=0 时，等式成立；又vr＞0，由式（8）可知V˙（qe）≤0。 令 V（qe）=0，得超曲面方程为，对 m（qe）求导，得，故（qe）不恒为零。 当‖qe‖→∞ 时，有 V（qe）→∞，由李雅谱诺夫第二法可知控制律（5）是全局渐近稳定的。

2.2 声呐避障算法设计

根据Khatibd的人工势场法模型[10]，可知障碍物对机器人的斥力Foi为

式中：ko为斥力增益系数：loi为第i个声呐与障碍物的距离：los为障碍物的影响距离。

声呐避障原理如图2所示，由5个声呐分别产生的斥力为Fo1-Fo5，则5个斥力的斥力和Fo为

式中：Fox为斥力和X轴的分量；Foy为斥力和Y轴的分量。

图2 声呐避障Fig.2 Sonar obstacle avoidance

同时，由目标点Pr以及机器人的坐标，可得目标点对机器人的引力及其各分量为

式中：ka为引力系数。由斥力和以及引力，可以计算合力Ft及其各分量，即

斥力系数ko和引力系数ka均采用单输入单输出系统。其中，输入变量分别为障碍物距离loi和目标点距离li，并且定义其5个模糊子集为｛距离很近，距离近，距离中，距离远，距离很远｝；输出变量分别为斥力增益系数ko和引力增益系数ka，模糊子集为｛增益很小，增益小，增益中，增益大，增益很大｝。该模糊系统的模糊规则见表1。

表1 模糊规则Tab.1 Fuzzy rule

为了将人工势场法和轨迹跟踪结合，在此引入虚拟目标点Pr′，使机器人受到的合力等效为虚拟目标点 Pr′对机器人的引力，于是由式（13）和式（14）可得

从而，可得虚拟目标点 Pr′（xr′，yr′）的坐标，即

在机器人的避障过程中，为了利用轨迹跟踪控制律，需要在机器人坐标和虚拟点之间进行插值，插值方法采用线性插值，并且限制插值点的距离。

避障中插值原理如图3所示，原相邻之间的距离lr为

当机器人在障碍物的影响范围之内时，在机器人坐标 M（x，y）和虚拟目标点 Pr′（xr′，yr′）坐标之间进行线性插值，插值节点xini分别为

图3 避障中插值Fig.3 Interpolation in the obstacle avoidance

而当机器人在障碍物的影响距离之外时，此时，应使机器人迅速回归轨迹，因此在 M（x，y）和Pr+1（xr+1，yr+1）点之间进行线性插值，插值节点分别为

根据插值节点可得 Pin1，Pin2，Pin3，Pin4的坐标。利用机器人坐标以及插值点可以计算避障时候的vr，ωr。

3 试验验证

3.1 Matlab仿真

假设给定的参考轨迹为

令影响距离los=1.6 m，并且2个模糊系统的输入均采用梯形隶属函数，输出为三角形隶属函数，并采用面积重心法去模糊化方法。调节系数kx=1，ky=0.9，kθ＝1.2。 机器人的初始位姿为 q=（-1，0，3）T，利用Matlab进行仿真。图4为李雅普诺夫函数V和跟踪误差 qe=[xe，ye，θe]T随时间的变化，如图可见跟踪误差能够快速趋于零。参考轨迹及机器人的轨迹如图5所示。

在坐标为（4，5），（5，0），（-5，-2）的位置上分别放置3个障碍物，则机器人轨迹以及参考轨迹如图6所示。从仿真结果可以看出，机器人能够有效避开障碍物并继续进行跟踪任务。

图4 李雅普诺夫函数及位姿误差Fig.4 Lyapunov function，error of position and posture

图5 参考轨迹和机器人的轨迹Fig.5 Reference trajectory and robot trajectory

图6 加入障碍物后的机器人轨迹Fig.6 Robot trajectory with obstacle

3.2 机器人试验

试验中，使用编译软件VC 6.0，编程语言为C++。机器人采用里程计定位。将尺寸为0.37 mm×0.2 mm×0.3 mm的纸箱作为障碍物放置在坐标（5，-1）的位置上，机器人及障碍物如图7所示。

机器人的初始位姿为qe=[0，0，0 ]T，调节系数kx=0.051，ky=0.060，kθ＝0.065。在 MFC 的窗口中画出参考轨迹、机器人轨迹以及实时检测出的障碍物，如图8所示。从MFC窗口可以看出试验效果较为理想。

图7 机器人及障碍物Fig.7 Robot and obstacle

图8 MFC窗口中机器人轨迹Fig.8 Robot trajectory in MFC window

4 结语

轨迹跟踪控制是移动机器人运动控制的重要问题。针对非完整轮式移动机器人运动学模型，提出了轨迹跟踪控制律，该控制律方法计算简单，收敛速度快，具有很好的稳定性。同时，根据人工势场法提出了虚拟目标点，并与轨迹跟踪结合，充分发挥了轨迹跟踪和人工势场法的优点，使器人既能跟踪轨迹又能有效避开障碍物。所研究的问题对今后的同时轨迹跟踪与避障问题具有一定的参考价值。

[1] Hou Z G，Zou A M，Cheng L，et al.Adaptive control of an electrically driven nonholonomic mobile robot via backstepping and fuzzy approach[J].IEEE Transactions on Control Systems Technology，2009，17（4）：803-815.

[2] Das T，Kar I N.Design and implementation of an adaptive fuzzy logic-based controller for wheeled mobile robots[J].IEEE Transactions on Control Systems Technology，2006，14（3）：501-510.

[3] Rossomando F G，Soria C，CarelliR.Neuralnetwork-based compensation control ofmobilerobotswith partially known structure[J].Iet Control Theory& Applications，2012，6（12）：1851-1860.

[4] Fierro R，Lewis F L.Control of a nonholonomic mobile robot using neural networks[M].IEEE Press，1998.

[5] Yue M，Hu P，Sun W.Path followingofaclassofnonholonomic mobile robot with underactuated vehicle body[J].Iet Control Theory&Applications，2010，4（10）：1898-1904.

[6] Fukao T，Nakagawa H，Adachi N.Adaptive tracking control of a nonholonomic mobile robot[J].IEEE Transactions on Robotics&Automation，2000，16（5）：609-615.

[7] Park B S，Yoo S J，Jin B P，et al.Adaptive neural sliding mode controlofnonholonomic wheeled mobile robotswith model uncertainty[J].IEEE Transactionson ControlSystems Technology，2009，17（1）：207-214.

[8] Khatib O.Real time obstacle avoidance for manipulators and mobile robots[J].International Journal of Robotics and Research，1986，5（1）：90-98.

[9] Chen X，Jia Y，Matsuno F.Tracking control of nonholonomic mobile robotswith velocity and acceleration constraints[J].American Control Conference，2014，18（6）：880-884.

[10]张殿富，刘福.基于人工势场法的路径规划方法研究及展望[J].计算机工程与科学，2013，35（6）：88-95.

Research on Trajectory Tracking and Obstacle Avoidance of Wheeled Mobile Robot

ZHANG Li-yang，CHEN Yi-mei

（College of Electrical Engineering and Automation，Tianjin Polytechnic University，Tianjin 300387，China）

Aiming at the problem of trajectory tracking and obstacles avoidance of nonholonomic wheeled mobile robots.An trajectory tracking control law is proposed on the basis of the differential equation of the tracking error，and its stability is determined by Lyapunov.Combined with the fuzzy control method，the traditional artificial potential field algorithm is improved.The coefficients of repulsion and gravitation are changed in real time，and virtual target points are extracted.Interpolate the adjacent tracking point and combine trajectory tracking control law，in this way can create maximum value of trajectory tracking and artificial potential field algorithm to realize trajectory tracking and obstacle avoidance tasks at the same time.In conclusion，the simulation and experimental results validate the effeciveness of this control law.Robot can track the trajectory and can avoid obstacles effectively.

wheeled mobile robot（WMR）；trajectory tracking；artificial potential field algorithm；virtual target point

TP242

A

1001-9944（2017）11-0072-05

10.19557/j.cnki.1001-9944.2017.11.017

2017-06-08；

2017-09-04

张立阳（1990—），男，硕士研究生，研究方向为机器人控制；陈奕梅（1972—），女，博士，副教授，研究方向为机器人控制技术。