双舵轮自动导引车轨迹追踪控制算法

齐嘉晖， 吴耀华， 汪 威

(山东大学控制科学与工程学院,济南 250061)

自动导引车(automated guided vehicle,AGV)是集成传感器技术、运动控制等技术于一体的综合控制系统,已广泛应用于物资运输、工业生产等诸多领域,成为智慧物流、智能制造和柔性生产中的重要设备。

自动导引车按照驱动方式分类,可以分为差速驱动导引车和舵轮驱动导引车。而舵轮式自动导引车由于驱动轮数量不同,常分为单舵轮自动导引车、双舵轮自动导引车与多舵轮自动导引车。其中双舵轮AGV由于车体运动灵活且应用场景丰富,值得重点研究。

能够精准、高效地完成轨迹跟踪任务控制是AGV系统的关键性能指标,也是AGV系统的研究热点之一。中外学者对AGV轨迹跟踪问题的研究较多,且多采用横向或纵向纠偏控制实现AGV轨迹跟踪。针对差速自动导引车,罗哉等[1]提出了一种基于最优偏差路径的模糊PID(proportional-integral-derivative,比例-积分-微分)纠偏算法,可以实现跟踪精度为3.2 mm,横向偏差在5 mm范围内的纠正,但位姿偏差的获取方式影响了系统鲁棒性；郑炳坤等[2]提出了一种基于模糊自整定PD(proportional-derivative，比例-微分)调节器来动态跟踪AGV轨迹,可实时监控车载的运行状态、跟踪控制信息,但在转弯时需要较大幅度降低速度；郭景华等[3]提出一种基于遗传算法的无人车横向模糊控制策略,但是实时性不高,且需要经过大量训练。针对单舵轮自动导引车,Ibari等[4]基于李雅普诺夫稳定性理论,设计了实现单舵轮AGV轨迹追踪的反演控制器,保证了跟踪误差稳定性；熊中刚等[5]提出基于免疫模糊PID的智能路径控制方法,实现了小型农机在复杂环境下,直线与曲线的轨迹跟踪。针对双舵轮自动导引车,Hemami[6]分析前后舵轮转角间关系,提出了低速行驶下车辆轨迹追踪方案；Yuan等[7]基于反演法设计了一种针对双舵轮半挂牵引车的轨迹追踪控制器,控制前后舵轮沿着统一轨迹行驶,提升了车辆的机动性。以上模型均包含较多参数,且参数值选取方法仅针对特定场景,难以实现推广。在快速路径跟踪时,AGV可能出现调整不及时而大幅走偏。针对四轮全向自动导引车,张业等[8]提出一种结合主动悬挂的四轮转向集成控制系统,运用模型预测控制算法,以车体各项性能指标为控制目标,满足极限工况下车体四轮转向的稳定性要求。Pandu等[9]提出了一种基于反步法的轨迹跟踪控制器来跟踪给定的轨迹。Nguyen等[10]设计一种滑动模式动态控制器,使自动导引车的速度收敛于速度控制输入,实现AGV匀速跟踪轨迹。

在轨迹跟踪控制方面,目前常见的控制器模型主要有PID控制器、前馈-反馈控制器、线性二次型调节器[11]及模型预测控制器等。其中,模型预测控制(model predictive control, MPC)是通过搭建系统模型,预测系统未来状态量和输出量,对控制量进行滚动优化,选择最佳控制行为的算法[12],主要应用于非线性高耦合的复杂系统控制中。致力于解决更长时间跨度的轨迹跟踪问题,Felipe等[13]提出一种具有非完整约束的AGV控制方案,采用模型预测控制直接处理约束。将误差模型连续线性化后,使用二次规划求模型预测控制。Pacheco等[14]对基于MPC的轨迹跟踪控制器与基于PID的轨迹跟踪控制器进行对比,结果显示在保证响应速度的基础上,MPC控制器具有更好的跟踪效果。

迄今为止,对双舵轮AGV的轨迹跟踪研究还不够深入。双舵轮AGV按底盘结构分类,主要分为两种,分别为舵轮中线布置结构与舵轮对角布置结构。其中,双舵轮驱动、激光导引,底盘采用舵轮中线布置的六轮AGV,具有操控性强、性价比高的优点,但对其轨迹跟踪控制研究甚少。双舵轮自动导引车的纠偏过程与车体结构、纠偏算法、控制系统设计等有密切的联系。现以舵轮中线布置双舵轮AGV为研究对象,研究其轨迹跟踪控制。其底盘由前后并置的两台舵轮机构和分列四角的4只从动万向轮组成。在建立其运动学模型的基础上,求得AGV最小转向半径,并重点分析该AGV转向优势。然后设计基于模型预测的双舵轮AGV纠偏控制器,结合控制系统设计,采用MATLAB软件仿真AGV纠偏控制过程,验证纠偏算法的实时性和鲁棒性。

1 AGV转弯运动学模型

为衡量单舵轮AGV与双舵轮AGV在转弯性能上的具体差异,首先建立两者的转弯运动学模型,在此基础上,分别计算转弯半径进行比较。在建立运动学模型时,均假设AGV车体结构为刚体,即符合刚体平动原理。同时,忽略AGV车身悬架对车体质心影响。

1.1 AGV转弯运动模型分析

单舵轮AGV由前轮单独提供驱动力,后轮提供平衡支撑。其中,运动方向和速度控制由舵轮控制的前轮来完成。车体结构示意图如图1(a)所示。

图1 单舵轮AGV示意图Fig.1 Single steering wheel AGV’s schematic

图1中,Of为前舵轮中心点,Oc为后轮连线中心点。转弯过程如图1(b)所示,车体以C点为圆心做半径为Rc的转弯运动。设后轮中心点与前舵轮的距离OcOf长为Lc。则在三角形OcCOf中,有:

双舵轮AGV的运动系统由4个万向轮和两个驱动舵轮组成,前后两个舵轮分别安装在车体前进方向轴线上。AGV的转弯和直线运动主要通过舵轮控制实现,可以按照预定的路线行进。

研究对象为图2(a)所示的双舵轮AGV。AGV车体四角各为一个从动轮,沿中线前后并置两轮为独立驱动的驱动轮。由于车体运动主要是通过动态调整前后舵轮来实现的,因此,在不影响运动学建模分析的情况下[15],将AGV的运动模型简化为前后驱动轮的运动模型。

图2 双舵轮AGV示意图Fig.2 Double-steering wheel AGV schematic

双舵轮AGV的结构示意图如图2(a)所示,每台舵轮驱动带动一个驱动轮。通过调节前后舵轮舵角,AGV可以完成直线路径与曲线路径的轨迹跟踪任务。

建立平面直角坐标系{XOY},Of、Ob分别为前后驱动轮与地面接触点。轴距为L。考虑AGV车身处于转弯圆弧轨迹时,由于圆弧轨迹各处曲率相同,因此行进过程中前后驱动轮转角相同。设前驱动轮与后驱动轮行进方向与机器人体坐标系{X′CY′}X轴夹角为θf、θb,Vf、Vb为前后轮行进速度,转弯半径为R。从后驱动轮进入转弯圆弧开始计时,经过t1时间,前轮走过的轨迹弧长为2Rθf。则有:

即驱动轮行进速度Vf恒定的前提下,车体转角2θf与其行进时间t1成正比。根据转弯轨迹三角关系,可知:

结合式(1)和式(3)可知,在航向角θf满足θf∈(0,90°]时,始终有R

1.2 双舵轮AGV定位算法

如图2(b)所示,设车体几何中心(X0,Y0)为前舵轮中点Of和后舵轮中点Ob的连线中点,在负载均匀的情况下,认为几何中心(X0,Y0)即为AGV车体质心,其初始值为(X0c,Y0c)。θf、θb分别为AGV前后轮转角角度，Vf、Vb为前后轮线速度。设O点为小车转弯时瞬时运动轨迹圆心,则Rf、Rb为前后轮转弯过程中的轨迹半径。则其前后舵轮的位置坐标可表示为

质心坐标C=(X0,Y0)可以表示为

(X0,Y0)即为AGV车体位置坐标。

质心在全局坐标系下的线速度状态量为

即线速度vc可表示为

其航向角α为

考虑刚体运动特性,车体绕O点运动时,前后舵轮中心点及车体质心C运动的在地面平动的角速度一致。车体运动过程中,结合舵轮运动速度及航向角,根据正弦定理得其角速度为

2 基于模型预测控制的双舵轮AGV纠偏控制

假设车轮与地面纯滚动接触,即AGV在平面运动时没有滑动,可以给出系统运动学模型如下：

进一步,双舵轮自动导引车运动学方程如式(11)所示:

由于自动导引车系统为非线性系统,对于非线性系统,求解最优控制的问题最终被转化为求解Hamilton-Jacobi-Bellman方程[16]。考虑非线性约束的存在,直接求解方程来获得精确解析解难以实现。由于可以将最优控制问题的求解转化为实时数值解有约束优化问题,大大降低了计算复杂度,提高预测控制算法计算速度,满足控制器实时性要求,因而采用MPC处理约束系统最优控制是一种可行的办法。

2.1 双舵轮AGV轨迹跟踪误差模型

为规划AGV跟踪路径,设立运行参考轨迹,参考轨迹任意点均满足状态空间表达式,即

即

结合状态空间表达式,可得:

将式(13)与式(12)做差,可得:

式(15)中：x′=x-xr表示车体实际运动位置与参考位置的状态误差；u′=u-ur则是控制输入量的误差。式(16)为连续模型,通过使用前向差分近似x′,得到以下AGV运动线性时变离散误差模型:

式(17)中：线性时变矩阵分别为

式(18)中：T为采样周期；k为采样时间。

非线性不完整系统是完全可控的,它可以在有限时间内,通过使用有限输入,从任何初始状态转向任何最终状态。很容易看出,当AGV静止时,关于静止操作点的线性化是不可控制的。但是,只要控制输入u不为零,这种线性化就变得可控[17-19]。这意味着用线性MPC跟踪参考轨迹是可行的。

2.2 轨迹跟踪控制器模型

离散化误差模型在系统k时刻增量式表达式为

Δu(k)=u(k)-u(k-1) (20)

则运动学模型的状态空间表达式为

式(2)中：

(22)

这里假设有限预测时域为p,在时域[τ,τ+p]内对系统状态进行预测。控制时域为c,在时域[τ,τ+c]内生成系统控制序列。所以预测时域内系统的预测方程为

Y(k)=φpψ(k)+QcΔU(k) (23)

式(23)中：Y(k)为系统输出量；φp为状态量参数；Qc为控制量参数；ΔU(k)为控制增量序列,用公式表达为

在系统预测方程中,需要建立准确的目标函数求解控制增量ΔU(k),从而得到控制时域[τ,τ+c]内合适的控制量序列,这里建立目标函数为

目标函数既保证了模型精准跟踪目标轨迹,又保证控制量符合AGV实际性能限制。式(25)中：Q为预测时域权重因子；R为控制时域权重因子；ρ为权重系数；γ为松弛变量。在目标函数引入松弛变量是为了便于在更大的可行域内寻求解。对松弛变量的选取不宜过大,否则模型即使具有较好的实时性,却不能保证较高的精度[20]。

AGV实际运行中,考虑AGV行驶可行域、舵轮转角和行进速度等现实约束条件,轨迹跟踪控制器模型可描述为

2.3 轨迹跟踪控制器模型算法求解

为获取系统最优控制序列,将上述最优化轨迹跟踪问题转化为带约束线性二次规划(quadratic programming, QP)问题进行求解。其形式如下[16]:

将预测方程式(23)代入目标函数式(26),经过整理可得:

式(28)中：G为控制增量系数矩阵；Ek为预测时域[τ,τ+p]内轨迹跟踪误差。针对带有线性约束的二次实函数,采用具有通用性的内点法求解,求得[τ,τ+c]内控制输入增量为

ΔUt=[Δut,Δut+1,…,Δut+c-1]T(29)

将该控制序列中第一个元素作为实际输入的控制量增量,通过计算得到控制量作用于实际系统,即

u(t)=u(t-1)+Δut(30)

当系统进入下一采样周期后,系统重新计算控制输入增量,循环迭代实现双舵轮AGV轨迹跟踪控制。

3 仿真与结果分析

为验证上述双舵轮AGV模型预测控制器的有效性,基于MATLAB平台,设计了轨迹跟踪仿真实验,具体流程如图3所示。

图3 双舵轮AGV轨迹跟踪系统仿真流程Fig.3 Simulation of double steering wheel AGV trajectory tracking system

在轮式自动导引车实际运行中,其控制量约束为

umin≤u≤umax(31)

控制增量约束为

Δumin≤Δu≤Δumax(32)

其中:

车辆初始位置:

仿真试验中轮式机器人运动模型和MPC控制器的默认参数如表1所示。

表1 仿真试验默认参数值Table 1 Simulation test default parameters value

由于轮式自动导引车在运动过程中,运动轨迹可分解为直线与圆弧两种路径。其中,车体行驶在弧线轨迹间稳定跟踪的能力是轨迹追踪算法鲁棒性的重要体现。为验证双舵轮AGV在模型预测控制算法下的轨迹跟踪效果,分别选取圆形路径和大曲率正弦曲线路径作为目标轨迹,进行跟踪仿真实验。首先分析系统控制量权重矩阵R以及采样周期N对MPC控制器的追踪效果的影响,以选取合理的模型参数。

3.1 模型参量对轨迹跟踪影响分析

模型预测控制算法实时性和鲁棒性受模型参数影响。实验选取圆形路径进行跟踪仿真,研究控制时域权重和预测时域大小对模型轨迹跟踪效果影响。

3.1.1 控制时域权重矩阵对系统影响分析

在目标函数中,控制时域权重矩阵R影响系统控制量的平滑性,从而影响轨迹跟踪的稳定性。为目标路径设定轮式机器人线速度为0.4 m/s；角速度0.074 rad/s；预测时域p=5。令控制时域权重矩阵R=λI2×2,其中λ∈{0.05,0.1,1,5,10},分别进行对比实验。图4(a)显示出不同控制时域权重作用下,控制器轨迹跟踪效果均较为理想。其中,图4(b)～图4(f)为不同权重值时,质心位置及航向角追踪偏差。结果显示,当控制时域权重R=λI2×2较小时,轨迹跟踪初始阶段误差较小,而运行中后期稳态跟踪误差近似。控制时域权重矩阵对系统的影响主要体现在轨迹跟踪初始阶段,当系统运行稳定后影响较小。

图4 不同控制时域权重矩阵轨迹跟踪结果Fig.4 Tracking results of different control time domain weight matrix

3.1.2 预测时域对系统影响分析

预测时域反应系统对未来输出的预测能力,预测时域长度显著影响轨迹跟踪效果。受限于硬件系统计算能力,预测时域较长时,模型求解时间过长,影响系统的快速动态性能。因此需要针对MPC轨迹跟踪算法单独优化预测时域,通过系统仿真选出合适参数,达到实时性与稳定性的平衡。

实验中轮式机器人速度为0.4 m/s；角速度为0.074 rad/s,预测时域N满足N∈{5,6,7,8},结果如图5所示。在合理范围内增大预测时域,轮式自动导引车取得了更好的稳态跟踪效果。其中,图5(a)反映了舵轮的初始阶段线速度输入量变化情况,增大N会显著改善稳态误差,但较长的预测时域增大了系统超调量。图5(b)反映了舵轮航向角输入量变化情况,当采样周期小于7时,机器人舵轮转角的角速度满足机器人性能限制,否则,舵角变化超过系统最大承受角速度0.2 rad/s。

图5 不同控制时域权重矩阵轨迹跟踪结果Fig.5 Tracking results of different control time domain weight matrix

3.2 MPC控制器与PID控制器对比实验分析

为验证基于模型预测控制的轨迹跟踪算法,设计对比仿真实验,比较PID控制器[14]和MPC控制器应用于双舵轮自动导引车进行轨迹跟踪效果。通过前述实验分析MPC模型参数变化对轨迹跟踪效果的影响,取采样周期N=7,控制时域权重矩阵R=0.1I2×2。

评估车体转向能力是衡量轨迹跟踪算法稳定性的常用方法。为此,将参考路径设置为直线与大曲率正弦曲线结合的S形弯道。其中,正弦曲线轨迹满足:

对比仿真实验结果如图6所示。

图6 PID控制器与MPC控制器跟踪效果对比Fig.6 Comparison between PID controller and MPC controller tracking effect performance

由图6(a)、图6(b)可知,针对曲率较大的圆弧,基于PID控制器的导引车跟踪轨迹会发生较大幅度超调,MPC控制器则表现出较为优良的控制特性。当运行时间处于33.7 s时,车体第一次到达曲率最大值处。此时,已有稳定运行趋势的PID控制器轨迹再次出现单向19 cm的较大超调量,而MPC控制器轨迹误差自18 s后,始终维持在[-1,1.1]范围内。

由图6(c)、图6(d)可知,当车体运动一段时间后,会迅速稳定在一定范围内达到跟踪指标要求。使用PID控制器的双舵轮AGV达到稳态时间较长,且稳态误差较大。

通过上述实验结果可知,在常规圆形路径以及曲率较大的正弦路径中,基于双舵轮AGV的MPC轨迹跟踪控制器均展现了良好的快速性与稳定性。模型预测控制器在选取合理参数后,其轨迹跟踪性能要优于常规PID算法。

4 结论

以纵向轴线安装的双舵轮自动导引车为研究对象，为研究双舵轮AGV转向性能,建立了自动导引车转向运动模型。同时,针对该双舵轮自动导引车,设计了一种基于模型预测控制的轨迹跟踪算法,使自动导引车快速稳定完成轨迹追踪任务。最后,通过MATLAB平台仿真实验验证了模型预测控制轨迹跟踪技术应用于双舵轮自动导引车的可行性和有效性。通过研究不同模型参数条件下控制器的实际跟踪效果,优化得到较为满意的控制器参数。同时,通过仿真实验将基于比例-积分-微分控制器的轨迹跟踪路线与参数优化后的模型预测控制器轨迹跟踪路线进行对比,验证了模型预测控制算法具有更高的稳定性。本文的AGV控制器只考虑了均匀负载情况,下一步将考虑载荷和惯性的影响。