基于积分模型预测控制的全向移动机器人轨迹跟踪研究①

颜佳晴 鲁聪达 蔡颖杰 彭 翔 潘 婷 周圣云

(*浙江工业大学机械工程学院 杭州310023)

(**深圳怡丰机器人科技有限公司 深圳518116)

0 引言

近年来移动机器人的应用越来越广泛。从港口的集装箱装卸、电子商务包裹分拣、智能化仓储到智能停车,移动机器人开始进入人们的生活并改变了生活方式。因此,移动机器人越来越受到研究人员的关注。目前,移动机器人的研究内容主要包括模型、控制、导航定位、避障、路径规划、点稳定等。[1-5]

移动机器人的控制是指通过输出控制率来驱动移动机器人按照预设的路径行驶[6]。模型预测控制(model predictive control,MPC)作为一种现代智能控制,是移动机器人控制算法的研究热点。基于运动学模型的预测控制策略已经应用于移动机器人和自动驾驶汽车的路径跟踪[6-8]。广义预测控制被用来解决路径跟随问题[9]。在这些研究中,预测控制仅输出角速度,而线速度则是通过特殊的速度发生器产生。随着研究的深入,当今的模型预测控制不仅是简单的控制速度和角速度,越来越多的研究集中在如何优化MPC 控制器上。例如,文献[10]通过将协调器添加到MPC 控制器,可以通过线性返回移动机器人中的跟踪误差来选择合适的权重参数。文献[11]考虑到摩擦力对MPC 控制器的影响,设计了一种模型预测控制器进行摩擦补偿,以达到较好的控制效果。

在移动机器人领域,解决摩擦补偿是一个重要的问题。在摩擦补偿的研究中,提出了许多控制方法,提出了模型技术控制中的各种摩擦模型。这些方法可以分为基于模型的技术[12-14]和基于非模型的技术。在基于模型的补偿方案中,使用静态或动态摩擦模型来缓解由于摩擦效应而导致的性能下降。在基于非模型的补偿方案中,在控制设计中不使用摩擦模型。摩擦补偿是通过适当选择控制增益参数或使用非基于模型的观测器来实现的[15]。但是,基于摩擦模型的模型预测控制的计算量十分巨大,不能满足实际应用中可靠、稳定、快速响应的要求。非模型的补偿方案中解决摩擦的非线性影响,是基于改进PID 控制器。除了需要精确的模型外,这些技术还具有补偿摩擦以外干扰的优势[11]。但是移动机器人在移动中除了摩擦以外,还有其他外部干扰所形成的稳态误差影响控制器的性能。为了消除模型预测控制中的稳态误差并减少处理器的计算量,并在移动机器人的跟踪控制中获得令人满意的性能,本文设计了一种具有积分作用的模型预测控制器,称为积分模型预测控制(integral model predictive control,IMPC)。本文的主要工作概括如下。

(1)设计具有积分作用的模型预测控制器。通过在传统模型预测控制器中添加积分作用,消除MPC 中的稳态误差,并获得更好的轨迹跟踪效果。

(2)在分析四差速全向移动机器人运动特性基础上,对差速驱动单元进行运动学分析,建立了完整的四差速全向移动机器人的运动学模型。

(3)将设计的IMPC 控制器应用于四差速全向移动机器人,与传统MPC 控制器进行对比测试。

1 全向移动机器人运动学分析

1.1 差动驱动模块的运动学分析

全向移动机器人的测试平台如图1 所示。与传统全向移动机器人相比,该测试平台使用差速驱动作为驱动形式,通过两个轮子的速度差实现驱动模块的直行或转弯。全向移动机器人通过4 对差速驱动单元的共同协作实现移动机器人的全向运行,因此称为四差速全向移动机器人。

图1 全向移动机器人的测试平台

差速驱动模块在ti和ti+1两个时刻的位姿如图2所示。差速驱动单元左右两个驱动轮之间的距离为l,左轮速度为vl,右轮速度为vr,两个相邻时刻所转过的角度为θ,d为右轮比左轮多行驶的距离。

图2 差速模块相邻两个时刻的位姿

通过几何分析得到差速驱动模块左右轮的速度与其中心点线速度v和角速度ω之间的关系:

式(1)表示通过差速驱动单元左右驱动轮的速度,可以计算得到差速驱动单元参考点的线速度和角速度。式(2)表示通过差速驱动单元参考点的线速度和角速度,可以计算得出驱动单元的左右轮速。

1.2 全向移动机器人整体分析

图3 为四差速全向移动机器人的整体结构和坐标系。移动机器人4 组差速驱动单元均匀分在车体四周,每组驱动单元有2 个驱动轮。X-Y为全局坐标系,XR-YR为局部坐标系。设全局坐标系与局部坐标系之间的夹角为φ。对差速驱动进行运动学分析,得出单舵轮驱动与差速驱动可以通过式(1)和式(2)实现vl、vr与v、ω之间的相互转换,因此差速驱动等效为舵轮驱动。

图3 四差速全向移动机器人的整体结构和坐标系

假设四差速全向移动机器人2 个前部驱动模块的速度与转角相同,2 个后部驱动模块的速度与转角也相同。根据假设,移动机器人前部2 个差速驱动单元用一个单舵轮替换,用A表示;同理,移动机器人后部2 个差速驱动单元用一个单舵轮替换,用B表示;C表示移动机器人的运动质心(参考点),A、B两点到C点的距离分别由lf和lr表示。φ表示移动机器人车头方向,V表示质心的速度,速度V与车辆轴线所形成的角度用β表示。简化后的四差速全向移动机器人模型如图4 所示,此模型称为单车模型。

图4 单车模型

通过对单车模型的几何关系分析,得到:

因为移动的机器人的角速度表示为θ,由此可得出移动机器人角速度为

联立式(3)和式(4),可以得出:

因此,单车模型的运动学方程表示为

式(6)为四差速全向移动机器人的运动学模型。3 个输入量分别是δf、δr和V。式(7)为β角的计算公式,移动机器人的航向角φ可由定位模块提供。

2 积分模型预测控制器设计

积分模型预测控制器的设计主要分为2 部分。第1 部分是MPC 控制器根据参考位姿pr=[xr,yr,θr]T与p=[x,y,θ]T移动机器人当前位姿之间的误差来输出的线速度和角速度。第2 部分是积分作用设计,用于补偿移动机器人运行中摩擦等稳态误差。IMPC 的结构如图5 所示,外部虚线框为IMPC,内部虚线框为传统MPC。

图5 积分模型预测控制器结构图

2.1 模型预测控制器设计

移动机器人运动学方程的一般式可表示为

其上面的每一点都满足上述的运动学方程,用r表示参考量,可表示为

其中,Xr=[xr,yr,φr]T,ur=[vr,ωr,βr]T。

在任意点(Xr,ur) 处泰勒展开,只保留一阶项,忽略高阶项,得到:

将式(10)减去式(9),得到:

式(11)称为移动机器人的线性误差模型,对式(11)进行离散化处理,得到:

采用式(13)作为目标函数。

式中,Np为预测时域,Nc为控制时域。

为了满足式(13)目标函数的形式,需将式(12)做一定的转换:

得到一个新的状态空间表达式:

经过推导,可以得到系统的预测输出表达式:

将式(16)带入目标函数式(13),可得到完整形式的目标函数表达式。

控制量的表达式表示为

控制增量的表达式表示为

通过对控制量增量的约束代替对控制量的约束,因此在设计约束条件时也应针对控制增量进行设计,以满足目标函数的要求。因此,需要对式(18)进行转换,求得相应的转换矩阵。

控制量与控制增量之间关系为

其中,1Nc为行数为Nc的列向量,Im为维度为m的单位矩阵,⊗为克罗内克积,u(k -1) 为上一时刻的控制量。

结合式(19)、式(20)和式(21),式(17)可改写为

其中,Umax和Umin分别为控制时域内的控制量最大值与最小值。

将控制量增量式目标函数转化为二次型形式,结合约束条件,转化为式(22)的优化问题。

对式(23)进行求解,可得到一系列控制量增量,如式(24)所示。

将式(24)中的第1 个元素作为控制增量输入系统,可得到:

进入下一个控制周期后,不断重复以上过程,实现四差速全向移动机器人的轨迹跟踪控制。

2.2 积分作用设计

实际应用中,由于移动机器人存在运行中的摩擦等外部因素影响,整个系统存在稳态误差。因此,通过对累积误差增加积分作用,即补偿移动机器人的摩擦和其他外部影响,使移动机器人获得更好的响应性和鲁棒性。

积分作用可累积历史误差,但传统的积分控制在启动时会在很短的时间内对系统输出造成较大的偏差,很容易引起积分累积,导致控制量超过临界值。该值对应于执行器允许的最大动作范围,这会导致较大的过冲甚至震荡。使用改进的积分器校正移动机器人的航向角误差和位置误差,最后将角速度输入到移动机器人。系统启动时,将使用模型预测控制来使系统稳定。当控制量接近给定值时,即当移动机器人的当前点接近目标点时,加入积分作用消除稳态误差,提高控制系统精度和鲁棒性。

积分作用表示为

移动机器人的误差由航向误差和位置误差组成,得到:

本文通过预测模型、滚动优化和反馈矫正3 个步骤计算,得出了传统模型预测控制器的控制量uM(t)。

式(28)表示当前控制量等于上一时刻的控制量加上控制量的增量。在加入积分作用后,得到:

本文通过积分作用校正移动机器人的航向角误差和位置误差,系统启动时,将使用模型预测控制来使系统稳定,当控制量接近给定值时,即当移动机器人的当前点接近目标点时,采用积分控制消除稳态误差,根据所设定的积分器的作用时间,将式(29)进行进一步优化。

式中,eref表示误差临界值。至此,式(30)为本文所设计的积分模型预测控制器。

3 实验

3.1 积分模型预测控制器实验设计和过程

图6 是进行轨迹跟踪实验测试的场地。测试场地长约35.5 m,宽约16 m,地面平整,无较大的起伏和缝隙,摩擦系数较高。在场地柱子周边和沿墙壁一侧都安装有激光反射柱,为激光导航提供定位信息。

图6 测试场地

图7 是通过激光定位模块所绘制出的测试场地激光特征图,定位模块可以将环境中的绝大部分信息扫描进地图,例如反光板、墙面、柱子等。在移动机器人进行定位时,定位模块可以根据当前位置信息与所扫描的特征地图进行匹配,进而确定移动机器人的当前位置,提供给移动机器人准备的位置信息。

图7 测试场地特征图

实验前确定全向移动机器人在环境中的当前位置,找到与当前位置最近的参考点并初始化。通过调试个人电脑可以给移动机器人下发本地任务,移动机器人根据任务信息搜索最近路径并开始移动,在移动过程中会不断记录当前位置与理论位置之间的位置误差和航向角误差,输出外部文本文件。积分模型预测控制器则会不断优化输出,不断减小机器人行进中的误差。

将设计的积分模型控制器应用于四差速全向移动机器人平台进行轨迹跟踪测试,验证四差速移动机器人平台的可行性,也验证积分模型预测控制在实际移动机器人运动轨迹跟踪控制中的有效性。在本文测试实验中设计3 种不同的轨迹进行测试,分别是直线、圆角矩形和“8”字形,综合分析移动机器人模型和模型预测控制算法的性能,图8 所示为移动机器人正进行圆角矩形轨迹跟踪测试。

图8 移动机器人圆角矩形测试

3.2 积分模型预测控制实验数据分析

3.2.1 直线轨迹

直线轨迹跟踪是最基本的轨迹跟踪问题之一。在移动机器人的移动过程中,能够以最快的速度运行,而直线运行时发生的抖动也最容易观察到。因此,IMPC 控制的稳定性和鲁棒性通过直线路径验证。

在直线轨迹测试中,移动机器人以1.2 m/s 的固定速度自主行进了14.5 m 长的直线路径。图9为两种算法在直线测试中的跟踪结果,实线为参考轨迹,虚线为IMPC 跟踪轨迹,点线为MPC 跟踪轨迹,从图9 中观察到两种算法都能顺利按照预设轨迹行进。通过对直线跟踪结果进行局部放大,观察到两种算法的跟踪效果,IMPC 跟踪轨迹与MPC 跟踪轨迹相比,更加靠近参考轨迹,即误差更小。图10和图11 显示了IMPC 和MPC 在直线路径上的横向误差和航向误差的比较,其中点划线为MPC 误差,实线为IMPC 误差。从图10 和图11 中明显观察到,IMPC 算法能够大大减小移动机器人行驶中的横向误差和航向误差。在直线运行时,传统的MPC控制器的横向误差稳定在-10 mm,航向误差稳定在0.7°。在加入积分作用后,IMPC可以大幅减小系统的稳态误差,横向误差在0 mm 附近波动,航向误差在0 °附近波动。

图9 直线测试跟踪结果

图10 直线测试横向误差

图11 直线测试航向误差

3.2.2 圆角矩形

在圆角矩形测试中,移动机器人以1 m/s 的平均速度自动行进36 m 长的圆角矩形路径。图12 为两种算法在相同路径下的跟踪结果,从图12 中观察到两种算法都能顺利按照预设轨迹行进。通过对圆角矩形左上部分进行局部放大,观察到两种算法的跟踪效果,其中实线为参考轨迹,虚线为IMPC 跟踪轨迹,点线为MPC 跟踪轨迹,IMPC 跟踪轨迹与MPC跟踪轨迹相比,更加靠近参考轨迹。图13 和图14显示了IMPC 和MPC 在圆角矩形路径上的横向误差和航向误差的比较,其中点划线为MPC 误差,实线为IMPC 误差。很明显,当运行一个圆角矩形时,IMPC 的横向误差稳定在5 mm 左右,航向误差稳定在0.2 °左右,因此IMPC 具有更好的跟踪性能。

图12 圆角矩形测试跟踪结果

图13 圆角矩形测试横向误差

图14 圆角矩形测试航向误差

3.2.3 8 字曲线

在8 字形曲线测试中,移动机器人以0.8 m/s的速度自动行驶28 m 长的8 字形路径。图15 显示了8 型路径实验的IMPC 和MPC 控制器的路径跟踪结果,其中图中实线为参考轨迹,虚线为IMPC 跟踪轨迹,点线为MPC 跟踪轨迹。通过对跟踪结果的局部放大,观察到IMPC 算法的实际路径更接近参考路径。图16 和图17 显示了IMPC 和MPC 在圆角矩形路径上的横向误差和航向误差的比较,其中点划线为MPC 误差,实线为IMPC 误差。通过图16 和图17 观察到,IMPC 控制器可以显着减少运动过程中移动机器人的航向误差和横向误差,并且误差波动较小、驾驶更加稳定。加上积分作用后,航向误差的最大值为0.5 °,最小误差为0 °。横向误差的最大值为-4.7 mm,最小误差为-1.1 mm。平均误差为0.2 °和-3.2 mm。

图15 8 字形测试跟踪结果

图16 8 字形测试横向误差

图17 8 字形测试航向误差

4 结论

本文提出了一种积分模型预测控制方法,并以四差速全向移动机器人作为研究对象,对移动机器人的轨迹跟踪控制进行了研究分析,利用机器人学、运动学、控制理论和计算机学等理论知识,结合大实车实验等综合性研究,设计出了跟踪性能强、鲁棒性好的轨迹跟踪系统。首先,建立运动学模型并线性化非线性移动机器人模型;其次,设计积分模型预测控制,包括MPC 设计和积分作用设计;最后,测试了直线、圆角矩形和8 字形3 个不同轨迹的实验。实验结果表明,与传统MPC 相比,IMPC 的横向误差和航向误差较小,实际路径更接近参考路径,具有更好的跟踪性和稳定性。