基于跟踪微分器的移动机器人轨迹规划与跟踪控制研究

叶伯生，谭帅，黎晗，潘钊，唐永杰

(华中科技大学机械科学与工程学院，湖北武汉 430074)

0 前言

移动物流机器人(AGV)凭借快速、高效等特点正逐渐取代人工，引导传统物流向智能物流转变。受新冠肺炎疫情影响，少接触和无接触的办公模式更是加快了AGV的部署速度。两轮差速AGV因低成本优势，在仓储物流行业得到了广泛的应用。但两轮移动机器人非完整性和强耦合性的特点，使其运动控制难度增加，这也使得其运动规划与控制算法成为移动机器人领域的研究热门。在运动规划方面，为了降低机器人的控制难度，规划简单的直线圆弧组合轨迹是常见的做法，但该方法存在减速转向问题，效率较低，因此现有研究多聚焦于规划曲线轨迹以提高机器人效率。常用的曲线包括贝塞尔曲线、正弦曲线和多项式曲线。贝塞尔曲线控制灵活，保证轨迹平滑的同时能根据环境的动态特性实时修改初始轨迹；但现有规划方式普遍存在速度过度依赖轨迹形状，难以参数化修改的问题。正弦曲线和多项式曲线过渡节点间处理困难，启停过程中易发生加速度突变。在控制算法方面，常见的有模糊PID控制、Backstepping控制、自适应神经网络控制、滑模控制、模型预测控制。模糊PID控制、Backstepping控制和自适应神经网络控制在控制稳定后均能获得较好的控制效果，但在实验中常用于以匀速跟踪圆或正弦轨迹，启停过程存在速度突变，位姿误差易受速度影响。滑模控制常存在抖动，采用补偿方式可有效削弱抖动，但可能出现速度阶跃，引入较大误差。模型预测控制每次迭代都能获得最优解，可以获得较优的轨迹跟踪性能，但运算量大且可能出现无解，因此控制系统的实时性和可靠性较难保证。

综上，本文作者一方面利用三次贝塞尔曲线拟合光滑轨迹，并通过跟踪微分器离散化轨迹，获得最大速度和最大加速度约束下的平滑速度曲线，解决了移动机器人轨迹规划和速度不连续问题；另一方面，在建立机器人位姿误差模型的基础上研究了机器人的Backstepping跟踪控制方法，解决了机器人轨迹跟踪控制问题；最后，将轨迹规划算法与控制算法结合，通过规划速度平滑和可参数化调整的轨迹，降低轨迹对控制器性能的影响，保证控制器平稳地跟踪曲线，提高控制精度。

1 轨迹规划

1.1 参考轨迹与参考速度规划

AGV在执行货物搬运任务前，调度系统将可用路径点序列的坐标信息下发给控制系统，机器人根据接收到的路径点序列进行轨迹规划。图1表示AGV物流机器人将货物从点搬运到点所规划的直线圆弧轨迹和曲线轨迹。为提升AGV货物搬运效率，采用基于贝塞尔曲线的机器人轨迹规划方法。阶贝塞尔曲线的表达形式为

图1 AGV货物搬运过程

(1)

(2)

为满足起点和终点姿态的独立约束条件，至少需要三阶贝塞尔曲线生成轨迹。更高阶的贝塞尔曲线虽然提升了轨迹的平滑性与灵活性，但硬件的运算负担也随之加剧，不利于实时控制。综上，以生成轨迹所需的最低阶贝塞尔曲线为研究对象，采用三阶贝塞尔曲线规划机器人平面运动轨迹。由式(1)(2)得到参考轨迹的计算如下：

(3)

atan2(1-0,1-0)、atan2(3-2,3-2)分别表示机器人起点和终点的姿态。

求解曲线长度，以规划机器人速度。平面中贝塞尔曲线的总长度可以运用积分方法进行计算：

(4)

其中：′()、′()为式(3)对求导的结果。式(4)的积分表达式通常难以获得，因此采用数值法求取曲线的长度。将贝塞尔曲线的控制参数均匀细分为份，应用Simpson公式求解曲线长度：

(5)

(6)

AGV的缓慢启停和整个运行过程的速度平滑是保证货物安全运送的关键，让机器人在最大速度和最大加速度允许范围内进行工作是机器人稳定运行的前提。针对上述要求，本文作者提出将自抗扰中获得巨大成功的跟踪微分器进行改造，将其运用到轨迹、速度和加速度受约束条件下的机器人轨迹规划中。该算法有两种实现方案：若不考虑加加速度约束，则只需改变最大速度和最大加速度两个参数就能参数化修改机器人速度曲线，实现降速和提速，但存在速度不连续问题；若考虑加加速度约束，则不仅能实现轨迹的参数化修改，还能保证整个速度曲线的平滑连续，因此该方案是本文作者的研究重点。上述两种方法通过跟踪微分器对长度为的贝塞尔曲线进行速度规划的过程分为跟踪微分和积分两个过程。两种方案参考速度的计算方法如下：

算法 1：不考虑加加速度约束

(7)

算法2：考虑加加速度约束

(8)

其中:fhan的函数表达式如下：

fhan(,,,)=

(9)

式(7)(8)为两种算法积分过程的数学描述；式(9)为跟踪微分过程的数学描述。其中：、、分别表示最大速度、最大加速度和最大加加速度；表示参考加加速度；代表控制周期；代表第个离散点；()、()、()分别代表·时刻所对应的轨迹长度、参考速度和参考加速度；sign为符号函数。

1.2 参考位姿与参考角速度规划

经过第1.1节的推导已经得到了参考速度()，为了完成轨迹规划，还需要规划机器人参考角速度()和参考位姿(()，()，())。为此，需获得轨迹长度()与贝塞尔曲线控制参数()的映射关系。为便于实时控制，采用Newton插值法快速获得()对应的()，再通过()离散化轨迹，获得对应的位姿。因轨迹长度关于控制参数单调递增，因而对于任意()，有且仅有一个()与之对应。参考角速度与参考位姿的计算过程如下：

步骤1,由式(10)初始化参数()；

步骤2,由式(5)计算(())；

步骤3,当′()≠0时，已知曲线上任意一点对应的轨迹长度()，可由式(11)的Newton迭代公式更新参数()；

步骤4,由式(11)对()进行迭代，得到()对应的精确解()；

步骤5,将()代入式(3)中的贝塞尔曲线方程，实时更新参考位置(()，())；

步骤6,由式(12)，计算参考角度和参考角速度。

(10)

(11)

()=atan2((),())

(12)

2 机器人运动学模型

文中的研究对象由两个万向轮和驱动轮分别提供支撑和驱动，万向轮对机器人运动的影响可以忽略，因此机器人的运动学模型可简化为图2所示的两轮差速模型。模型中：-为参考坐标系(世界坐标系)；-为机器人坐标系；点(点)表示AGV的质心；为两轮之间的距离，l、r分别表示机器人的左、右轮；、分别代表机器人参考速度和参考角度；、、分别表示机器人质心实际速度、角速度和角度，它们相对于参考系-描述；、表示位置误差，表示姿态角误差，它们都相对于坐标系-描述。

图2 机器人简化运动学模型

运动过程中，机器人两轮速度和质心速度满足如下约束关系：

(13)

其中:为车轮半径。由图2中的机器人运动学模型建立误差间的坐标变换关系，得到机器人位姿误差方程：

(14)

其中:、表示机器人质心位置。将上式对时间求导，并由防滑约束关系得到位姿误差的微分形式：

(15)

3 控制器设计

基于Backstepping方法，设计AGV轨迹跟踪控制器：

步骤1，由位置误差定义如下Lyapunov函数：

(16)

≥0，仅当、项均为0时等号成立。将各项对时间求偏导，联立式(15)得到如下关系式：

(17)

根据Lyapunov稳定性判据，为满足项的稳定性条件，线速度的控制率取值如下：

=+cos

(18)

式中：>0。为满足项的稳定性条件，建立变量间的虚拟约束关系，引入如下虚拟控制量：

(19)

令=，考察项的稳定性变为判断sin()≤0是否成立。求导知该不等式恒成立，且仅当=0时等号成立。联立式(17)(18)(19)得：

(20)

步骤2，为实现姿态角跟踪，定义如下误差变量：

=-

(21)

结合Backstepping思想，定义如下Lyapunov函数：

(22)

≥0，仅当、均为0时等号成立。进一步对各项进行求导，得到如下关系：

(23)

(24)

(25)

(26)

(27)

整理上面的表达式，最后得到如下关系：

(28)

由上式知的跟踪性能只与相关，的跟踪性能只与相关，是个二阶系统，其跟踪性能与频率和阻尼相关。将式(28)化简为二阶微分方程的一般形式：

(29)

4 仿真及实验验证

搭建如图3所示的AGV实物平台和仿真平台，验证轨迹规划算法和控制算法的正确性。为保证实物平台与仿真平台轨迹跟踪效果的一致性，将仿真平台中机器人的运动学和动力学参数设置成与AGV实物一致。机器人的长、宽、高分别为1 000、 750、300 mm；轮子半径=90 mm，两轮中心的横向距离=711 mm。

图3 实验平台

4.1 仿真

4.1.1 大误差下轨迹跟踪仿真

为了测试算法的可靠性，实验中人为地将机器人参考轨迹起点和实际轨迹的起点设置在相差较大的位置。仿真在V-REP软件中进行，由Newton物理引擎计算，坐标均以mm为单位计量。仿真时用于轨迹规划的4个控制点的坐标分别为：(0,0)、(2 100,3 200)、(4 900,800)、(7 000,4 000)。考虑不同初始状态下，机器人轨迹收敛情况不一致，为此设计了机器人在4种不同初始状态下的仿真实验。4个实验中，机器人起点分别位于世界坐标系的4个象限中，且每次相对于坐标原点的偏移量都是机器人机身的长度和宽度。4次实验的起点坐标分别为(-750,-1 000)、(750,1 000)、(-750,1 000)、(750,-1 000)，姿态角均为0 rad。仿真时采用第1.1节中的算法2进行轨迹规划，跟踪微分器的相关参数分别设置为=0.05 s、=600 mm/s、=500 mm/s、=1 200 mm/s。控制器的相关参数分别取为=8.2、=0.5。实验结果如图4所示。

图4 轨迹规划及轨迹跟踪仿真实验效果

由图4(a)可知：即使起点存在较大的位姿误差，在控制作用下机器人均能逐渐向规划的轨迹收敛，并完成轨迹跟踪。观察图4(b)的参考速度可知：规划的速度曲线能满足速度和加速度的约束，说明了跟踪微分器进行轨迹规划的可行性。观察图4(b)中的速度跟踪曲线可知:由于起点误差的原因，在加速阶段为了减小跟踪误差，机器人实时对自身速度进行调整，出现实际速度偏离规划速度的现象；到达匀速阶段后，误差不断减小，机器人实际速度与规划速度逐渐贴合，并最终完成速度跟踪。仿真结果验证了轨迹规划方法的可行性和控制器的可靠性。

4.1.2 小误差下轨迹跟踪仿真

机器人实际运行速度曲线能证明轨迹规划的有效性，误差曲线能反映控制器的跟踪控制性能。为此，设计了两组对照实验。第一组对照实验是机器人未规划速度曲线与用跟踪微分器进行速度规划的轨迹跟踪对比实验；第二组对照实验是采用滑模控制器与文中设计的控制器分别对机器人进行轨迹跟踪控制的控制效果对比实验。仿真实验中，用于控制生成贝塞尔曲线的4个控制点的坐标分别为：(0,0)、(646,2 290)、(3 878,4 122)、(6 463,4 580)；跟踪微分器与控制器的参数设置与第4.1.1节中一致；未规划速度的轨迹跟踪实验中，参照文献[17]不单独对速度进行规划，将机器人参考速度设置为1 200 mm/s；滑模控制器的结构形式参照文献[12]中的VSC控制器进行设计。实验中，机器人初始位置误差均为0，初始姿态角误差均在0.035 rad以内。仿真结果如图5所示。

分析图5中各组实验的速度曲线和位姿误差曲线，未进行速度规划时，机器人在启停阶段存在速度阶跃，导致产生较大的位姿误差；采用滑模控制器进行控制时，机器人在向规划轨迹收敛的过程中容易产生速度波动，导致出现严重的颤振现象；采用文中的方法进行控制时，机器人实现了以平滑连续的速度在接近零的误差范围内跟踪期望轨迹。该结果表明：文中的轨迹规划方法与控制器设计的有效性和可靠性。

图5 实验对比

4.2 实验

实验主要是为了验证控制器的有效性与仿真结果的正确性。实验在由1 000 mm×1 000 mm正方形网格组成的铝板上进行，机器人实时位姿由里程计和惯导经融合后解算获取。实验过程中，为了同时验证控制器跟踪存在速度不连续轨迹的有效性，采用不考虑加加速度约束的轨迹规划方案，该方案规划的轨迹在加减速阶段仍存在加速度突变。实验中贝塞尔曲线的4个控制点坐标分别为：(1 996,7 016)、(2 046,7 758)、(2 298,8 352)、(2 500,8 500)。控制器的参数与第4.1.1节中设置的一致。跟踪微分器的参数分别设置为=0.02 s、=600 mm/s、=1 000 mm/s。实验开始前，机器人质心在地面上的投影点位于处，车头指向的切线方向。实验结果如图6所示。

观察图6中的位姿误差曲线，整个轨迹跟踪过程中方向误差在±20 mm区间内波动，方向误差在±10 mm区间内波动，角度误差接近0。分析图6中的实验结果，整个轨迹跟踪过程中，机器人均能稳定跟踪规划的轨迹，即使在存在速度突变的加减速阶段，机器人仍能够较好地跟踪参考速度和参考轨迹，这说明了控制器的可靠性，证明了文中所设计的控制算法的有效性。

图6 轨迹规划及轨迹跟踪效果

方向出现较大的误差可能是由于速度不连续导致加速度突变，产生了打滑。实际上，在推导机器人位姿态误差方程时，采用了防滑约束关系，因此若出现打滑将可能影响跟踪效果。分析方向的位姿误差曲线可知，机器人最大位置误差实际上是出现在机器人速度不连续的阶段，而这些阶段恰是最容易出现打滑的阶段。事实上，该实验进一步验证了引言中所讨论的问题，即轨迹规划不考虑速度连续问题会因为加速度突变导致跟踪误差倍增。

5 结论

同时研究了机器人的轨迹规划和跟踪控制问题。首先，提出利用贝塞尔曲线解决直线圆弧轨迹存在的剧烈转向问题；接着，提出了保证AGV安全稳定运行的约束条件，并将约束条件作为跟踪微分器的输入获得了平滑的参考速度曲线；然后，基于Backstepping方法进行了轨迹跟踪控制器的设计，并分析了控制器参数对跟踪性能的影响；最后，搭建了V-REP仿真实验环境和AGV实物实验环境对算法进行验证。相比于未进行速度规划、滑模控制和速度不连续等控制方式，文中提出的轨迹规划方法与控制算法在轨迹跟踪过程中，降低了轨迹对控制器性能的影响，提升了控制稳定性和跟踪精度。