CN111273551B - 双舵轮自动导引车轨迹追踪控制方法及系统 - Google Patents

Abstract

本发明公开了双舵轮自动导引车轨迹追踪控制方法及系统，包括：建立双舵轮自动导引车AGV运动学模型：基于上述模型，计算双舵轮自动导引车转弯半径；建立双舵轮前后舵轮的位置坐标即AGV车体位置坐标；通过计算得到控制量作用于双舵轮自动导引车，实现双舵轮AGV轨迹跟踪控制。

Description

双舵轮自动导引车轨迹追踪控制方法及系统

技术领域

本发明属于控制技术领域，尤其涉及双舵轮自动导引车轨迹追踪控制方法及系统。

背景技术

本部分的陈述仅仅是提供了与本公开相关的背景技术信息，不必然构成在先技术。

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

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

能够精准高效地完成轨迹跟踪任务控制是AGV系统的关键性能指标，也是AGV系统的研究热点之一。国内外对AGV轨迹跟踪问题的研究较多，且多采用横向或纵向纠偏控制实现AGV轨迹跟踪。

发明人在研究中发现，针对差速自动导引车，现有文献提出了一种基于最优偏差路径的模糊PID(proportional–integral–derivative)纠偏算法，可以实现跟踪精度为3.2mm，横向偏差在5mm范围内的纠正，但位姿偏差的获取方式影响了系统鲁棒性。现有文献提出了一种基于模糊自整定PD(proportional–derivative)调节器来动态跟踪AGV轨迹，可实时监控车载的运行状态、跟踪控制信息，但在转弯时需要较大幅度降低速度；现有文献提出一种基于遗传算法的无人车横向模糊控制策略，但是实时性不高，且需要经过大量训练。针对单舵轮自动导引车，现有文献提出基于李雅普诺夫稳定性理论，设计了实现单舵轮AGV轨迹追踪的反演控制器，保证了跟踪误差稳定性；现有文献提出基于免疫模糊PID的智能路径控制方法，实现了小型农机在复杂环境下，直线与曲线的轨迹跟踪。针对双舵轮自动导引车，现有文献分析前后舵轮转角间关系，提出了低速行驶下车辆轨迹追踪方案；现有文献提出基于反演法设计了一种针对双舵轮半挂牵引车的轨迹追踪控制器，控制前后舵轮沿着统一轨迹行驶，提升了车辆的机动性。以上模型均包含较多参数，且参数值选取方法仅针对特定场景，难以实现推广。

在快速路径跟踪时，AGV可能出现调整不及时而大幅走偏。针对四轮全向自动导引车，现有文献提出一种结合主动悬挂的四轮转向集成控制系统，运用模型预测控制算法，以车体各项性能指标为控制目标，满足极限工况下车体四轮转向的稳定性要求。现有文献提出了一种基于反步法的轨迹跟踪控制器来跟踪给定的轨迹。现有文献设计一种滑动模式动态控制器，使自动导引车的速度收敛于速度控制输入，实现AGV匀速跟踪轨迹。

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

发明人在研究中还发现，迄今为止，对双舵轮AGV的轨迹跟踪研究还不够深入。双舵轮AGV按底盘结构分类，主要分为两种，分别为舵轮中线布置结构与舵轮对角布置结构。其中，双舵轮驱动、激光导引，底盘采用舵轮中线布置的六轮AGV，具有操控性强、性价比高的优点，但对其轨迹跟踪控制研究甚少。

发明内容

为克服上述现有技术的不足，本发明提供了双舵轮自动导引车轨迹追踪控制方法，在建立其运动学模型的基础上，求得AGV最小转向半径，然后设计基于模型预测的双舵轮AGV纠偏控制器，纠偏算法具有实时性和鲁棒性。

第一方面，为实现上述目的，本发明的一个或多个实施例提供了如下技术方案：

双舵轮自动导引车轨迹追踪控制方法，包括：

建立双舵轮自动导引车AGV运动学模型：将AGV的运动模型简化为前后驱动轮的运动模型；

基于上述模型，建立双舵轮前后舵轮的位置坐标即AGV车体位置坐标，基于车体位置坐标，车体运动过程中，结合舵轮运动速度及航向角，得其角速度；

将轮式自动导引车系统位姿状态分别表示车体质心在全局坐标系下的位置坐标，将双舵轮自动导引车自身运动线速度与角速度作为控制输入量，以车体质心线速度和角速度表示，获得双舵轮自动导引车运动学方程；

求解双舵轮自动导引车运动学方程：将最优控制问题的求解转化为实时数值解有约束优化问题，通过计算得到控制量作用于双舵轮自动导引车，实现双舵轮AGV轨迹跟踪控制。

第二方面，为实现上述目的，本发明的一个或多个实施例提供了如下技术方案：

双舵轮自动导引车轨迹追踪控制系统，包括处理器，所述处理器被配置执行双舵轮自动导引车轨迹追踪控制方法的步骤。

第三方面，为实现上述目的，本发明的一个或多个实施例提供了如下技术方案：

双舵轮自动导引车，该双舵轮自动导引车按照上述双舵轮自动导引车轨迹追踪控制方法通过调节前后舵轮舵角，完成直线路径与曲线路径的轨迹跟踪任务。

以上一个或多个技术方案存在以下有益效果：

本发明的技术方案以纵向轴线安装的双舵轮自动导引车为研究对象。为研究双舵轮AGV转向性能，建立了自动导引车转向运动模型。同时，针对该双舵轮自动导引车，设计了一种基于模型预测控制的轨迹跟踪算法，使自动导引车快速稳定完成轨迹追踪任务，具有更高的稳定性。

本发明的技术方案采用该模型预测控制器可以满足各类约束条件，高效精准完成对连续大曲率目标路径的有效跟踪，同时具有较高的实时性和鲁棒性。

附图说明

构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

图1(a)-图1(b)为本发明实施例单舵轮AGV车体结构及转弯示意图；

图2(a)-图2(b)为本发明实施例双舵轮AGV车体结构及转弯示意图；

图3为本发明实施例双舵轮AGV轨迹跟踪系统仿真流程图；

图4(a)-图4(f)为本发明实施例不同控制时域权重矩阵轨迹跟踪结果示意图；

图5(a)-图5(b)为本发明实施例不同控制时域权重矩阵轨迹跟踪结果图；

图6(a)-图6(d)为本发明实施例PID控制器与MPC控制器跟踪效果对比图。

具体实施方式

应该指出，以下详细说明都是示例性的，旨在对本发明提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

本发明提出的总体思路：

本发明以舵轮中线布置双舵轮AGV为研究对象，研究其轨迹跟踪控制。其底盘由前后并置的两台舵轮机构和分列四角的四只从动万向轮组成。在建立其运动学模型的基础上，求得AGV最小转向半径，并重点分析该AGV转向优势。然后设计基于模型预测的双舵轮AGV纠偏控制器，结合控制系统设计，采用Matlab软件仿真AGV纠偏控制过程，验证纠偏算法的实时性和鲁棒性。

实施例一

本实施例公开了双舵轮自动导引车轨迹追踪控制方法，具体步骤包括：

建立双舵轮自动导引车AGV运动学模型：将AGV的运动模型简化为前后驱动轮的运动模型；

基于上述模型，计算双舵轮自动导引车转弯半径；

建立双舵轮前后舵轮的位置坐标，以及车体质心坐标即AGV车体位置坐标，基于车体位置坐标，车体运动过程中，结合舵轮运动速度及航向角，得其角速度；

将轮式自动导引车系统位姿状态分别表示车体质心在全局坐标系下的位置坐标，将双舵轮自动导引车自身运动线速度与角速度作为控制输入量，以车体质心线速度和角速度表示，获得双舵轮自动导引车运动学方程；

求解双舵轮自动导引车运动学方程：将最优控制问题的求解转化为实时数值解有约束优化问题，通过计算得到控制量作用于双舵轮自动导引车，实现双舵轮AGV轨迹跟踪控制。

在该实施例子中，双舵轮自动导引车的纠偏过程与车体结构、纠偏算法、控制系统设计等有密切的联系。

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

针对上述模型，单舵轮AGV由前轮单独提供驱动力，后轮提供平衡支撑。其中，运动方向和速度控制由舵轮控制的前轮来完成。其车体结构如图1(a)所示，单舵轮AGV转弯如图1(b)所示。

在图1(a)-图1(b)中，O_f为前舵轮中心点，O_c为后轮连线中心点。转弯过程如图1(b)所示，车体以C点为圆心做半径为R_c的转弯运动。设后轮中心点与前舵轮的距离O_cO_f长为L_c。则在三角形O_cCO_f中，有：

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

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

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

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

即驱动轮行进速度V_f恒定的前提下，车体转角2θ_f与其行进时间t₁成正比。根据转弯轨迹三角关系，可知：

结合式(1)和式(3)可知，在航向角θ_f满足θ_f∈(0,90°]时，始终有R<R_c。即AGV直角转弯过程中，双舵轮AGV始终保持较小转弯半径。同时，由于执行转弯任务时，双舵轮AGV可由前后舵轮配合实现转向，因此与单舵轮AGV相比，具有响应时间短的优点。

双舵轮AGV定位算法：如图1(b)所示，设车体几何中心(X₀,Y₀)为前舵轮中点O_f和后舵轮中点O_b的连线中点，在负载均匀的情况下，认为几何中心(X₀,Y₀)即为AGV车体质心，其初始值为(X_0c,Y_0c)。θ_f、θ_b分别为AGV前后轮转角角度V_f、V_b为前后轮线速度。设O点为小车转弯时瞬时运动轨迹圆心，则R_f、R_b为前后轮转弯过程中的轨迹半径。则其前后舵轮的位置坐标可表示为：

其中，质心坐标C＝(X₀,Y₀)可以表示为：

即为AGV车体位置坐标。

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

即线速度v_c可表示为：

其航向角θ为：

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

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

模型预测控制是一种通过对系统将来一段时间内控制输入的结果进行预测，优化目标函数从而得出最优控制量的优化控制策略。预测步骤即轨迹跟踪控制器模型设计思路。

1.设计预测模型。离散化误差模型在k时刻的增量式表达，在未来一段时域内预测系统状态量，得到系统控制序列，得到系统预测方程。

2.进行滚动优化，在系统的每个采样时刻，选取从采样时刻起将来的N个采样周期作为其预测时域，采用预测时域内系统状态量的偏差和控制量的偏差的离散二次型函数的和作为目标函数。

3.反馈修正，通过内点法求解，得到控制输入增量，将控制输入量第一个元素作为实际输入的控制量增量，实施输出量，循环迭代实现控制。

当应用模型预测控制纠正双舵轮AGV轨迹路径时，将轮式自动导引车系统位姿状态记为X＝[X₀ Y₀ θ]^T＝[x y θ]^T，x，y分别表示车体质心在全局坐标系{XOY}下的位置坐标。将双舵轮自动导引车自身运动线速度与角速度作为控制输入量u，以车体质心线速度和角速度表示，记为u＝[v_c ω_c]^T＝[v ω]^T。

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

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

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

双舵轮AGV轨迹跟踪误差模型：为规划AGV跟踪路径，设立运行参考轨迹，参考轨迹任意点均满足状态空间表达式(12)，即：

表示参考轨迹上任意点的状态空间表达式。

其中参考轨迹状态量为X_r＝[x_r y_r θ_r]^T，其参考输入控制量u_r＝[v_r,w_r]^T，(x_r,u_r)为参考轨迹车辆质心坐标，θ_r为参考航向角。为线性化预测控制模型，将式(12)在参考轨迹点(x_r,u_r)处进行泰勒展开，忽略高阶项，保留一阶项，可以获得双舵轮移动机器人线性化误差模型，其中，(13)式为参考轨迹保留一阶项的泰勒级数展开。线性误差模型为(16)式：

即：

结合状态空间表达式，可得：

将式(13)与式(12)做差，可得：

表示连续误差模型的状态空间表达式。

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

其中，线性时变矩阵分别为：

其中，T为采样周期，k为采样时间。

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

轨迹跟踪控制器模型：离散化误差模型在系统k时刻增量式表达式为：

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

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

其中，

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

Y(k)＝φ_pψ(k)+Q_cΔU(k) (23)

其中，Y(k)为系统输出量；φ_p为状态量参数；Q_c为控制量参数；ΔU(k)为控制增量序列，用公式表达为：

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

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

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

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

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

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

ΔU_t＝[Δu_t,Δu_t+1,…,Δu_t+c-1]^T (29)

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

u(t)＝u(t-1)+Δu_t (30)

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

实施例二

本实施例的目的是提供双舵轮自动导引车轨迹追踪控制系统，包括处理器，所述处理器被配置执行实施例子一中的双舵轮自动导引车轨迹追踪控制方法的步骤。

实施例三

本实施例的目的是提供双舵轮自动导引车，该双舵轮自动导引车按照上述实施例子一中的双舵轮自动导引车轨迹追踪控制方法通过调节前后舵轮舵角，完成直线路径与曲线路径的轨迹跟踪任务。

以上实施例二、三装置中涉及的各步骤与方法实施例一相对应，具体实施方式可参见实施例一的相关说明部分。术语“计算机可读存储介质”应该理解为包括一个或多个指令集的单个介质或多个介质；还应当被理解为包括任何介质，所述任何介质能够存储、编码或承载用于由处理器执行的指令集并使处理器执行本发明中的任一方法。

仿真与结果分析：为验证上述双舵轮AGV模型预测控制器的有效性，本文基于Matlab平台，设计了轨迹跟踪仿真实验，具体流程图如图3所示。在轮式自动导引车实际运行中，其控制量约束为：

u_min≤u≤u_max

控制增量约束为：

Δu_min≤Δu≤Δu_max

其中：

u_min＝[v_min ω_min]^T＝[-1.5 -1.2]^T

u_max＝[v_max ω_max]^T＝[1.5 1.2]^T

Δu_min＝[Δv_min Δω_min]^T＝[-0.35 -0.2]^T

Δu_max＝[Δv_max Δω_max]^T＝[0.35 0.2]^T

车辆初始位置：

X₀＝[X_0c Y_0c θ_0c]^T＝[0 0.5 0.1]^T

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

表1仿真试验默认参数值

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

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

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

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

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

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

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

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

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

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

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

本领域技术人员应该明白，上述本发明的各模块或各步骤可以用通用的计算机装置来实现，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。本发明不限制于任何特定的硬件和软件的结合。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (5)

1.双舵轮自动导引车轨迹追踪控制方法，其特征是，包括：

建立双舵轮自动导引车AGV运动学模型：双舵轮AGV转弯时，AGV车体四角各为一个从动轮，沿中线前后并置两轮为独立驱动的驱动轮；由于车体运动通过动态调整前后舵轮来实现的，因此，在不影响运动学建模分析的情况下，将AGV的运动模型简化为前后驱动轮的运动模型；

基于上述模型，建立双舵轮前后舵轮的位置坐标即AGV车体位置坐标，基于车体位置坐标，车体运动过程中，结合舵轮运动速度及航向角，得其角速度；

建立平面直角坐标系{XOY}，O_f、O_b分别为前后驱动轮与地面接触点；轴距为L；考虑AGV车身处于转弯圆弧轨迹时，由于圆弧轨迹各处曲率相同，因此行进过程中前后驱动轮转角相同；前驱动轮与后驱动轮行进方向与机器人体坐标系{X’CY’}X轴夹角为θ_f、θ_b，V_f、V_b为前后轮行进速度，转弯半径为R；从后驱动轮进入转弯圆弧开始计时，经过t₁时间，前轮走过的轨迹弧长为2Rθ_f；则有：

即驱动轮行进速度V_f恒定的前提下，车体转角2θ_f与其行进时间t₁成正比；根据转弯轨迹三角关系，可知：

双舵轮AGV定位算法：车体几何中心(X₀,Y₀)为前舵轮中点O_f和后舵轮中点O_b的连线中点，在负载均匀的情况下，认为几何中心(X₀,Y₀)即为AGV车体质心，其初始值为(X_0c,Y_0c)；θ_f、θ_b分别为AGV前后轮转角角度V_f、V_b为前后轮线速度；O点为小车转弯时瞬时运动轨迹圆心，则R_f、R_b为前后轮转弯过程中的轨迹半径；则其前后舵轮的位置坐标可表示为：

其中，质心坐标C＝(X₀,Y₀)可以表示为：

即为AGV车体位置坐标；

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

即线速度v_c可表示为：

其航向角θ为：

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

将轮式自动导引车系统位姿状态分别表示车体质心在全局坐标系下的位置坐标，将双舵轮自动导引车自身运动线速度与角速度作为控制输入量，以车体质心线速度和角速度表示，获得双舵轮自动导引车运动学方程；

当应用模型预测控制纠正双舵轮AGV轨迹路径时，将轮式自动导引车系统位姿状态记为X＝[X₀ Y₀ θ]^T＝[x y θ]^T，x，y分别表示车体质心在全局坐标系{XOY}下的位置坐标；将双舵轮自动导引车自身运动线速度与角速度作为控制输入量u，以车体质心线速度和角速度表示，记为u＝[v_c ω_c]^T＝[v ω]^T；

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

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

求解双舵轮自动导引车运动学方程：将最优控制问题的求解转化为实时数值解有约束优化问题，通过计算得到控制量作用于双舵轮自动导引车，实现双舵轮AGV轨迹跟踪控制；

双舵轮AGV轨迹跟踪误差模型：为规划AGV跟踪路径，设立运行参考轨迹，参考轨迹任意点均满足状态空间表达式(12)，即：

表示参考轨迹上任意点的状态空间表达式；

其中参考轨迹状态量为X_r＝[x_r y_r θ_r]^T，其参考输入控制量u_r＝[v_r,w_r]^T，(x_r,u_r)为参考轨迹车辆质心坐标，θ_r为参考航向角；为线性化预测控制模型，将式(12)在参考轨迹点(x_r,u_r)处进行泰勒展开，忽略高阶项，保留一阶项，获得双舵轮移动机器人线性化误差模型，其中，(13)式为参考轨迹保留一阶项的泰勒级数展开；线性误差模型为(16)式：

即：

结合状态空间表达式，可得：

将式(13)与式(12)做差，可得：

表示连续误差模型的状态空间表达式；

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

其中，线性时变矩阵分别为：

其中，T为采样周期，k为采样时间；

轨迹跟踪控制器模型：离散化误差模型在系统k时刻增量式表达式为：

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

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

其中，

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

Y(k)＝φ_pψ(k)+Q_cΔU(k) (23)

其中，Y(k)为系统输出量；φ_p为状态量参数；Q_c为控制量参数；ΔU(k)为控制增量序列，用公式表达为：

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

目标函数既保证了模型精准跟踪目标轨迹，又保证控制量符合AGV实际性能限制；Q为预测时域权重因子；R为控制时域权重因子；_ρ为权重系数；γ为松弛变量；

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

轨迹跟踪控制器模型算法求解：为获取系统最优控制序列，将上述最优化轨迹跟踪问题转化为带约束线性二次规划问题进行求解；其形式如下：

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

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

ΔU_t＝[Δu_t,Δu_t+1,…,Δu_t+c-1]^T (29)

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

u(t)＝u(t-1)+Δu_t (30)

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

2.如权利要求1所述的双舵轮自动导引车轨迹追踪控制方法，其特征是，所述双舵轮自动导引车运动学方程获得的前提是AGV在平面运动时没有滑动。

3.如权利要求1所述的双舵轮自动导引车轨迹追踪控制方法，其特征是，考虑AGV行驶可行域、舵轮转角和行进速度现实约束条件，以及目标函数及AGV运动线性时变离散误差模型，获得轨迹跟踪控制器模型。

4.双舵轮自动导引车轨迹追踪控制系统，包括处理器，其特征是，所述处理器被配置执行权利要求1-3任一所述的双舵轮自动导引车轨迹追踪控制方法的步骤。

5.双舵轮自动导引车，该双舵轮自动导引车按照上述权利要求1-3任一所述的双舵轮自动导引车轨迹追踪控制方法通过调节前后舵轮舵角，完成直线路径与曲线路径的轨迹跟踪任务。

Images