移动机器人路径跟踪的方法及研究进展

梁婕

广东技术师范大学 自动化学院，广东广州 510665

0 引言

在过去的几十年里，机器人技术已经成为工程研究中最重要的领域之一，机器人的使用也越来越广泛。此外，随着当前科学技术的发展进步，机器人不仅变得更便宜，而且更有效、更快、更灵活、更智能，也极大地方便了人们的日常生活。随着移动机器人的广泛应用，对于机器人的研究也进一步深入。机器人在传感器的作用下自动行驶，基本的操作过程主要是根据传感器的信息检测环境进行自身定位，建图、路径规划、路径跟踪及导航，其中路径跟踪是非常重要的一个方面[1]。

路径跟踪是移动机器人[2]的一项基本任务，近年来受到了广泛关注。路径跟踪的控制问题是设计移动机器人速度的控制律，使机器人的真实轨迹与参考轨迹之间的跟踪误差最小化。路径误差是由噪声、外部干扰和来自内部和外部源的测量传感器误差造成的[3]。

路径跟踪方面的研究主要包含了以下几个方面的内容：基于反馈误差的控制算法，该类型算法只关注了系统的输入输出，而忽略了机器人的运动规律，因此这种算法更适合于没有模型的系统；基于模型的控制算法，该类算法运用机器人的运动学模型以及动态模型来跟踪规划好的路径；基于参考路径和机器人之间的几何关系的控制器，实现起来较为容易，该算法利用运动模型计算移动机器人的转向角，优点是易于植入，且具有良好的鲁棒性。

本文对其中的PID 控制、MPC 控制、LQR 控制及纯追踪算法以及斯坦利算法的现状进行了归纳分析，几种方法的优缺点如表1 所示，为今后的研究提供一定的参考。

表1 几种方法的优缺点

1 理论模型

通常，移动机器人的路径跟踪系统由模型和控制器组成，根据运动学模型[4]对机器人进行建模，得到机器人的位置和航向角。路径跟踪控制器的目标是使移动机器人的横向误差和航向误差收敛到零，从而稳定到达目标位置。其中，横向跟踪误差是指机器人当前位置到参考路径最近点的偏差，航向误差是指机器人与参考路径之间的航向差。移动机器人的运动学模型如图1 所示。

该移动机器人[5]的轮子是由两个平行的驱动轮和一个万向轮组成，XOY为全局坐标系，XRCYR为局部坐标系，机器人的位姿由全局坐标系q=[x y θ]定义，其中，x和y是移动机器人在全局系统中的坐标点，θ为机器人的方向角度，C为机器人的中心，假设机器人没有滑动和滚动。式（1）为移动机器人的运动学模型[6]：

其中，v和ω分别为机器人的线速度和角速度；x和y为机器人在全局坐标系中的坐标点；θ为机器人的方向角度。

2 算法控制

2.1 PID 控制

PID 控制器主要包含3 个部分：比例、积分、微分。标准比例积分导数（PID）控制器因其结构简单、易于设计而在工业机器人领域得到了广泛的应用。PID控制器可以实现稳定性和跟踪控制，但跟踪控制存在静态误差，其性能与比例项Kp的比例因子有关，而且PID 控制器需要建立过程的数学模型参数。

PID 控制的基本思路[7]是计算被控对象的输入输出差值，通过比例调节、积分调节、微分调节得到系统的控制量，利用控制量对机器人进行控制，使其跟随设置好的路径进行行驶。PID 控制的整体控制量的计算方法[8-9]如式（2）所示。所有这3 个控制器组合在一起，使它给出一个控制信号。作为一种反馈控制器，它在指定的水平上传递控制输出，并保持输出，使过程变量与闭环任务的期望输出之间存在零误差。在PID 控制中，最重要的就是设置好Kp、Ki、Kd3 个参数，如果选取的参数合适，就可以达到所期待的效果。

其中，u(t)为控制器输出的控制量；Kp为比例系数；Ki为积分系数；Kd为微分系数；e(t)为实际值与目标值之间的差值。

PID 控制器被认为是一种广泛应用的控制器，在控制系统中应用范围很广，因为其增益参数易于调整，所以PID 控制器的优点是简单、稳定且易于实现[10]。但这种方法需要存储每一次的偏差，计算量较大，且存在非线性、不精确和参数不准确等问题，因此，PID控制器限制了系统的实现设计和性能，此外，PID 控制器的参数整定需要一定的经验和大量的实验。

SIDI M H A 等人[11]对履带式机器人进行建模并使用PID 控制器控制机器人进行跟踪。TIWARI T 等人[12]提出了将非耦合线性PID 控制器与显式条件相结合来控制移动机器人，提高了跟踪的性能。该研究展示了一种利用遗传算法（GA）进行调整的最优控制方法来定义模糊控制器的显式输入/输出范围。将遗传算法应用到改进的PID 控制上，提高了控制精度和收敛速度[13]。KARTHIKA B 等人[14]设计了SDRE 控制器和SDC 矩阵，提高了两轮自平衡机器人的平衡能力。文献[15]通过遗传算法对PID 控制参数进行整定，使系统稳定性得到提高。使用SSA 对PID 参数进行整定，优化后的控制器可以有效地完成路径跟踪[16]。经过FPGA 令移动机器人进行速度调节和基于PSO 的PID 算法进行智能方向调节，通过精细算法及控制系统优化，令移动机器人在工作中对路径具有高精准选择，提升移动机器人的工作效率[17]。采用遗传算法优化PID参数，使得参数选取更加灵活，跟踪性能更好[18]。

2.2 模型预测控制器

模型预测控制器（简称MPC）是一种基于模型优化的控制器，该控制器主要包含了3 个部分：模型预测、滚动优化、反馈校正。其中，最重要的部分是预测模型和优化函数。

通常，MPC 被表述为一个线性二次极小化问题。利用二次规划求解器可以有效地求解该公式化的问题。横向车辆引导任务被表述为一个沿参考曲线的约束最优控制问题，通过将系统动力学沿参考曲线进行线性化处理，推导出一个线性时变模型。性能准则由一个二次代价函数定义。

MPC 实现跟踪的基本思路：

（1）建立移动机器人的运动学状态方程；

（2）对非线性方程进行线性化处理；

（3）对模型进行离散化处理；

（4）建立预测方程，即将当前状态量和上一个时刻的控制量组合成新的状态量输入；

（5）构造合适的代价函数；

（6）求解最优问题。

MPC 具有良好的鲁棒性，且跟踪误差相对较小，但其收敛速度缓慢。

MPC 控制器的核心是在每个时间步长求解一个优化问题，从而找到一个最优的序列[19]。文献[20]基于细化路径和曲率，采用前后向积分策略设计了受轮胎-路面摩擦限制的最小时间纵向速度剖面。YUE X等人[21]提出了一种由模糊系统自适应调整参数的模型预测控制器，使机器人能够在斜坡上准确、稳定地跟踪给定路径。近年来，计算硬件的相当大的进步促进了模型预测控制和非线性优化技术的实际应用，例如自动驱动领域的差分动力学编程。一些研究人员探索了模型预测控制（MPC）和DDP 在机器人的自动驾驶和自动控制中的可能性[22]。SUN C 等人[23]提出带有切换跟踪误差的MPC 控制器，建立基于模糊逻辑的状态分类器，从而更准确地跟踪路径。

2.3 线性二次调节控制器

线性二次调节器（简称LQR）是一种利用状态空间方法分析系统的现代控制方法。其主要思想是通过优化代价函数式（3）来运行动态系统，找到一个控制函数，优化控制量，使得成本函数最小化。LQR 控制器采用状态反馈控制器，用一组线性微分方程来解释系统的动力学。LQR 实际上是一种流行的控制器，它通过反馈增益控制系统的闭环稳定性和提高系统的设计性能。LQR 由于其稳定性和鲁棒性等优点，被广泛应用于许多控制应用的系统设计中。其中，J为代价函数；Q、R分别为状态权重矩阵和控制权重矩阵；x为当前系统状态量；u为当前系统控制量。

LQR 实现路径跟踪的基本思路：

（1）选择参数矩阵Q和R；

（2）求解Riccati 方程得到矩阵P；

（3）计算系统反馈矩阵K=R-1BP；

（4）计算控制量u=-Kx。

式（4）为Riccati 方程：

其中，A、B、Q、R皆为已知量，可以求解出矩阵P。

LQR 算法在跟踪直线时性能良好，但在路径曲率变化过快时会存在超调问题。文献[24]采用预瞄PID对LQR 控制器进行转角补偿，从而消除稳态误差，使得跟踪精度更高。文献[25]提出一种基于路径跟踪误差的参数计算方法和一种基于车-路位置关系的参数调整规则，以此实现LQR 控制器的改进，提高控制器的适应性与控制精度。采用基于最优控制理论的线性二次调节器对机器人进行误差自适应调整，能够有效跟踪路径[26]。根据虚拟跟踪路径曲率、车辆车速和虚拟路径相对航向偏差设计自适应预瞄算法，以此建立自适应预瞄前馈LQR 控制器，具有较好的跟踪效果[27]。以侧偏角最小为目标设计转向控制规则。基于最优控制理论，采用线性二次型调节器实现车辆轨迹跟踪的最优控制[28]。

MPC 和LQR 都是基于模型预测的控制器，两种方法有很多相似之处，但也有很多不同的地方。第一，LQR 主要是针对控制中的线性系统，而MPC 的研究对象更广泛，既可以是线性系统，也可以是非线性系统；第二，因为两种方法都存在优化问题，所以对于目标函数的设计是必要的，LQR 的目标函数是研究对象的状态以及控制输入的二次型函数，而MPC 的目标函数是将优化目标与权重相乘再求和；第三，LQR 是将一个控制周期内求出的最优解发给控制器，而MPC是滚动优化，将最优解序列中的第一个控制值发送给控制器。

2.4 纯追踪算法

纯追踪算法是一种基于移动机器人和路径之间几何关系的方法，其主要思想是由几何关系的分析运动学模型和参考路径，以及这些模型跟踪的路径。几何路径跟踪方法主要有纯追踪算法以及斯坦利方法，已经得到了广泛而实际的应用。其主要优点是采用简单的几何模型，根据当前状态及时反馈，从而满足移动机器人的实时需求，实现简单，鲁棒性好。

纯追踪算法如图2 所示[29]，OXY为惯性坐标系，O为机器人左右轮的中心坐标，r为机器人的转向半径，ld为前瞻距离，(x, y)为前瞻点。

由几何关系[30]可得：

其中，x、y为前瞻点的横纵坐标；ld为前瞻距离；r为转向半径；γ为曲率。

圆弧曲率与前瞻距离的关系为：

阿克曼转向模型可得到机器人的前轮转角为：

其中，L为机器人的轴距；δ为机器人的转向角；r为机器人的转向半径；x为机器人与前瞻点之间的横向误差。

纯追踪算法的性能主要取决于前瞻距离的选择[30]，如果选择的前瞻距离过小，机器人的跟踪精度相对较高，但会导致跟踪不稳定，容易产生振荡；如果选择的前瞻距离过大，机器人跟踪相对稳定，路径会比较平滑，但是容易产生切角问题。由此可见，前瞻距离的选择对于移动机器人的路径跟踪至关重要，因此要合理选择前瞻距离。

纯追踪算法的步骤可以概括为以下几点：

（1）选择合适的前瞻距离；

（2）以机器人为中心画圆，将其与路径的交点记为前瞻点，也即目标点；

（3）计算移动机器人的偏转角度；

（4）控制机器人沿着路径跟踪；

（5）更新位置，重复上述几个步骤。

纯追踪算法实现起来较为简单，易于植入，且鲁棒性强[31]，但当机器人的速度发生显著变化时，该算法会产生较大的误差。

对于纯追踪算法的研究有很多，文献[32]给出了预瞄距离的确定方案，结合模糊理论设计出一种变预瞄距离的路径跟踪模糊控制器。AHN J 等人[33]提出了一种方法，通过考虑车辆和道路之间的关系启发式地选择一个有预见性的点，使用该方法可以稳定收敛于期望路径。文献[34]研究了基于车速和GPS 轨迹与当前车辆位置的最短距离的两次多项式函数的先进纯跟踪算法，该算法可以在车辆跟踪理想路径时标定车辆的航向角和转向角，减少横向误差。将速度作为一个计算前瞻距离的参数来提高系统的稳定性，将比例控制器添加到纯追踪转向角的计算算法，提高算法在曲线阶段的跟踪精度[35]。文献[36]提出了一种新的基于优化前瞻距离的纯追踪算法OLDPPA，通过实验，证明了该算法的有效性。文献[37]提出了一个更健壮的、有预见性的距离调优策略的几何路径跟踪控制器，控制器可以适应不同的速度，执行更稳定，证明了没有必要添加所有相关变量的模糊输入，以及两个变量是足以调整先行的距离。文献[38]将PID 与纯追踪算法结合起来可以减少横向角误差并输出稳定的控制角度。将纯追踪作为基本控制器，添加比例积分控制器，提高了转向的稳定性，并使用低通滤波器平滑最终的转向角[39]。将模糊控制器与纯追踪算法结合起来，速度与横向误差作为输入，前瞻距离作为输出，可以快速消除路径跟踪误差，提高精度[40]。文献[41]提出基于双切圆巡线与纯跟踪结合的路径跟踪算法，降低了收敛时间。张宜宝等人[42]设计改进型纯追踪模型并提出基于模糊PID 的转向控制方法。

纯追踪算法结构简单，有良好的鲁棒性，但过于依赖前瞻距离，虽然以上研究取得了一些进展，但前瞻距离与众多因素的关系难以描述，导致前视距离最优值难以获取，因而该算法多适用于速度较慢的路径跟踪。

2.5 斯坦利（Stanley）算法

斯坦利算法是基于横向跟踪误差的非线性反馈函数，利用移动机器人的位姿和参考轨迹之间的相对几何关系生成转向角指令，从而控制移动机器人向目标位置行驶。该算法的主要优点是不会出现如纯追踪算法那样的切角问题，但其只关注当前的一个状态，鲁棒性相对来说比较差。斯坦利算法示意图如图3 所示。

控制原理可表示为：

其中，δ为期望转角；δe为横向误差引起的期望转角；θ为航向偏差引起的期望转角。

若只考虑横向误差对期望转角的影响，忽略航向误差的作用，那么：

其中，k为增益系数；e为横向误差；v为机器人速度。

若只考虑航向误差对期望转角的影响，忽略横向误差的作用，那么：

其中，θ为航向偏差。

在计算期望转角时，需要综合考虑这两个方面的影响，由此得到前轮期望转角，如式（14）：

斯坦利算法的流程如下所述：

（1）得到当前时刻机器人的位姿信息；

（2）找到距离当前位置最近的点；

（3）获取最近点和当前位置的航向角，计算横向误差和航向误差；

（4）根据公式计算前轮转角；

（5）更新机器人位姿信息并进行循环。

将几何路径跟踪方法中的纯追踪方法与斯坦利方法结合[43]起来，可以有效地减少切角问题，更快地收敛到规划好的路径上。文献[44]提出了一种新的通过模糊控制和粒子群算法改进斯坦利算法的方法，修改控制增益自适应的跟踪误差，提高跟踪精度。WANG L 等人[45]将多种群遗传算法应用到斯坦利算法上，获得了更好的跟踪效能。文献[46]采用纯追踪算法对Stanley 算法中车轮转角进行改进，从而计算出合适的前轮转角，提高了跟踪的平滑性。文献[47]提出一种增益系数自适应的Stanley 模型路径跟踪算法，设计模糊推理和解模糊化过程，实时确定控制模型增益系数，提高Stanley 模型对不同曲率路径的自适应能力。

3 结论与展望

随着科技的发展进步，移动机器人技术已经得到了迅速发展。同时，机器人的广泛应用也引起越来越多的人研究与它相关的技术。本文主要关注的是控制算法。在性能标准方面，跟踪误差是评估控制器性能最常见的特征。根据控制点的位置和所使用的控制方法，跟踪误差的确定可能因研究的不同而不同。在回顾了几种算法后，可以得出结论：每一种控制算法都有其自身的优缺点，对于控制算法的设计应该将被控对象和应用场景考虑在内，针对被控对象使用合适的控制算法，设计相应的控制器。此外，控制器的精度和稳定性仍是研究的重难点，需要不断地进行完善，以达到更好的跟踪效果。目前，绝大多数的控制器都会使用两种或两种以上的算法以便为机器人的路径跟踪提供更好地性能，对以往算法的回顾可以帮助我们更好的进行新的工作。此外，将智能优化算法、机器学习及深度学习等领域的知识应用到运动控制领域也是一个可以深入研究的方向。