CN115933647A - 基于复合控制算法的omr轨迹跟踪控制方法及存储介质 - Google Patents

Abstract

本发明公开了基于复合控制算法的OMR轨迹跟踪控制方法及存储介质，方法包括：给定参考机器人的姿态和速度，通过标称运动学模型计算得到参考机器人的位置信息，通过实际运动学模型得到跟随机器人的位置信息；结合轨迹跟踪误差模型，计算得到第k时刻跟随机器人的跟踪误差；通过模型预测控制器获得跟随机器人的最优输入，结合改进滑模观测器，获得跟随机器人的真实集总扰动向量的估计值；通过估计值对跟随机器人进行扰动估计补偿，控制跟随机器人的速度。本申请提高了系统的容错率和鲁棒性，消除了抖振的影响，从而提高收敛速度；并且，消除了未知干扰和不确定性的影响，实现更精确的控制，降低了系统的误差。本申请应用于机器人控制领域。

Description

基于复合控制算法的OMR轨迹跟踪控制方法及存储介质

技术领域

本发明涉及机器人控制技术领域，特别涉及基于复合控制算法的OMR轨迹跟踪控制方法及存储介质。

背景技术

与传统的差分驱动机器人相比，配备四个麦克纳姆轮的全向移动机器人(OMR，Omnidirectional Mobile Robot)能够实现沿任意方向的平移而不旋转，且具有较好的稳定性，能够在狭小的工作空间中获得高精度的轨迹。因此，它们被广泛部署在拥挤的地区和紧凑的环境中，如仓库、走廊和船舱。然而，全向移动机器人具有紧密且复杂的轮系结构，其模型具有不确定性，此外运动过程中容易受到物理环境中的扰动影响。

轨迹跟踪控制是为了解决如何将移动机器人的实际位置和方向收敛到期望的参考轨迹的问题。该领域在多智能体协作如多机器人编队和多机器人轨迹规划等方面具有广阔的应用前景。针对全向移动机器人的轨迹跟踪控制，目前已存在如模糊控制、滑模控制和最优控制等控制算法，这些算法虽然能够有效地处理机器人的各种复杂约束问题，然而这些方法都没有考虑到全向移动机器人的物理限制，未充分考虑扰动的影响，在实际应用中可能较难达到预期的控制效果。

在本领域中，为了解决控制系统受未知干扰影响的问题，滑模观测器(SMO，Sliding Mode Observer)被普遍认为是一种较为有效的方法。基于SMO的控制算法可以提高系统的抗干扰能力，具有较好的控制性能。然而，传统的SMO模型在引入不连续符号函数时不可避免地导致系统出现高频抖振，影响系统的稳定性，进而使得系统的收敛速度较慢，控制精度低。

因此，对于本领域而言，在控制全向移动机器人时，如何抑制抖振的影响、外部干扰和模型不确定性的影响的同时，处理其物理约束，成为本领域亟需解决的技术问题。

发明内容

本发明的目的是提供基于复合控制算法的OMR轨迹跟踪控制方法及存储介质，能够提高了全向移动机器人的跟踪性能，有效地处理物理约束，消除未知干扰和不确定性的影响。

本发明解决其技术问题的解决方案是：一方面，本申请提供基于复合控制算法的OMR轨迹跟踪控制方法，包括如下步骤:

S1，建立标称运动学模型和考虑集总扰动的实际运动学模型，并构建轨迹跟踪误差模型，所述集总扰动包括OMR在运动时的未知干扰和其运动学模型的不确定性；

S2，针对OMR的轨迹跟踪问题，建立了轨迹跟踪误差模型，并在此基础上设计模型预测控制器；

S3，给定参考OMR的姿态和速度，通过标称运动学模型计算得到参考OMR的位置信息，通过实际运动学模型得到跟随OMR的位置信息；

S4，根据参考OMR的位置信息和跟随OMR的位置信息，结合轨迹跟踪误差模型，计算得到第k时刻跟随OMR的跟踪误差；

S5，通过模型预测控制器获得跟随OMR的最优输入，并根据最优输入和跟踪误差，结合改进滑模观测器，获得跟随OMR的真实集总扰动向量的估计值，根据估计值对跟随OMR进行扰动补偿，进而控制跟随OMR的运动；

S6，循环S3至S5。

另一方面，本申请提供了一种存储介质，其中存储有处理器可执行的指令，所述处理器可执行的指令在由处理器执行时用于执行所述的基于复合控制算法的OMR轨迹跟踪控制方法。

本发明的有益效果是：提供基于复合控制算法的OMR轨迹跟踪控制方法及存储介质，首先设计基于变幂次趋近律的改进型滑模观测器来抑制机器人在运动过程中的外部干扰和其运动学模型不确定性的影响，将改进滑模观测器与模型预测控制算法进行结合，得到一种适用于全向移动机器人的基于改进型滑模观测器的模型预测控制策略，控制策略对未知扰动进行逼近，从而实现扰动补偿。本申请的控制方法大大提高了系统的容错率和鲁棒性，消除了抖振的影响，从而提高收敛速度；并且，消除了未知干扰和不确定性的影响，实现更精确的控制，降低了系统的误差。

本申请的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本申请而了解。本申请的目的和其他优点可通过在说明书、权利要求书以及附图中所特别指出的结构来实现和获得。

附图说明

图1为本申请实施例提供的标称运动学模型的示意图；

图2为本申请实施例提供的全向移动机器人的结构示意图；

图3为本申请实施例提供的OMR的标称运动学模型的参数定义图；

图4为本申请实施例提供的OMR的实际运动学模型的参数定义图；

图5为本申请实施例提供的参考OMR的物理参数图；

图6为本申请实施例提供的跟随机器人的集总扰动向量的估计值的仿真图；

图7为本申请实施例提供的跟随机器人和参考机器人的轨迹跟踪结果图；

图8为本申请实施例提供的现有的MPC算法和本申请的VPSMO-MPC算法的矩形轨迹跟踪误差比较图；

图9为本申请实施例提供的OMR轨迹跟踪控制方法的控制原理图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本申请，并不用于限定本申请。

下面结合说明书附图和具体的实施例对本申请进行进一步的说明。所描述的实施例不应视为对本申请的限制，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

在以下的描述中，涉及到“一些实施例”，其描述了所有可能实施例的子集，但是可以理解，“一些实施例”可以是所有可能实施例的相同子集或不同子集，并且可以在不冲突的情况下相互结合。

除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中所使用的术语只是为了描述本申请实施例的目的，不是旨在限制本申请。

对本申请实施例进行进一步详细说明之前，对本申请实施例中涉及的名词和术语进行说明，本申请实施例中涉及的名词和术语适用于如下的解释。

(1)OMR(Omnidirectional Mobile Robot，全向移动机器人)，为配备四个麦克纳姆轮的全向移动机器人，其能够实现沿任意方向的平移而不旋转，且具有较好的稳定性，能够在狭小的工作空间中获得高精度的轨迹。因此，它们被广泛部署在拥挤的地区和紧凑的环境中，如仓库、走廊和船舱。

(2)MPC(Model controlled prediction，模型预测控制)，是一类特殊的控制。它的当前控制动作是在每一个采样瞬间通过求解一个有限时域开环最优控制问题而获得。过程的当前状态作为最优控制问题的初始状态，解得的最优控制序列只实施第一个控制作用。其本质上模型预测控制求解一个开环最优控制问题。

(3)SMO(Sliding Mode Observer，滑模观测器)，是基于滑模变结构控制方法的一种状态观测器。本申请实施例中提及的VPSMO是指本申请所设计的改进滑模观测器。

(4)VPSMO-MPC，是指基于改进型滑模观测器的模型预测控制策略，是本申请所设计的复合控制算法。

本发明应用的领域为全向移动机器人控制领域。全向移动机器人具有紧密且复杂的轮系结构，其模型具有不确定性，此外运动过程中容易受到物理环境中的扰动影响。轨迹跟踪控制是为了解决如何将全向移动机器人的实际位置和方向收敛到期望的参考轨迹的问题。目前，已存在有模糊控制、滑模控制和最优控制等模型预测控制算法来解决轨迹跟踪控制问题。如Z.Sun针对全向移动机器人的轨迹跟踪任务，设计了一种自适应积分终端滑模控制算法；G.Cao提出了一种模糊自适应比例积分微分控制方法，用于提高全向移动机器人的运动精度和稳定性；D.Zhang设计了抗外界干扰的一种近似最优跟踪控制器等等。虽然传统的模型预测控制算法能够有效地处理全向移动机器人的各种复杂约束问题，但传统的模型预测控制算法往往依赖于全向移动机器人的运动学模型，现有的全全向移动机器人大多仅使用标称模型，但全向移动机器人的运动学和动力学会受到干扰和不确定性的影响，进而导致大多数运动系统的控制性能也会受到可能存在于各种现实环境中未知干扰的影响。即，传统的模型预测控制算法并未充分考虑到全向移动机器人所存在的扰动，在实际应用中传统的模型预测控制算法可能会无法达到预期的控制效果。

为了解决控制系统受未知干扰影响的问题，在本领域中SMO(Sliding ModeObserver，滑模观测器)被普遍认为是一种较为有效的方法。根据已有大量的研究成果，基于SMO的控制算法可以提高系统的抗干扰能力，具有较好的控制性能。然而，传统的SMO模型在引入不连续符号函数时不可避免地导致系统出现高频抖振，影响了全向移动机器人系统的稳定性，且系统的收敛速度较慢。针对该问题，现有的研究中在SMO的设计中引入了高阶滑模观测器来逼近位置扰动，虽然高阶的SMO极大地提高了控制性能，但是只能在一定程度上消除抖振，并不能完全消除。

针对上述现有技术中所存在的问题，本发明提出一种适用于全向移动机器人的复合控制算法及存储介质，结合SMO和模型预测控制算法的优点，改进了SMO的缺陷，实现了对全向移动机器人的高效和高精度的轨迹跟踪控制。具体地，本发明首先设计了一种基于变幂次趋近律的改进型滑模观测器来抑制外部干扰和模型不确定性的影响，然后创新性地将改进型滑模观测器与模型预测控制算法结合，得到一种适用于全向移动机器人的基于改进型滑模观测器的模型预测控制策略(VPSMO-MPC)。本发明通过VPSMO-MPC算法控制全向移动机器人的运动，提高了全向移动机器人的跟踪性能，消除抖振的影响的同时，有效地处理了物理约束，消除了未知干扰和不确定性的影响。

本申请提供的OMR轨迹跟踪控制方法可以包括但不限于以下步骤。

S1，建立标称运动学模型和考虑集总扰动的实际运动学模型，并构建轨迹跟踪误差模型。

需要说明的是，集总扰动包括全向移动机器人在运动时的未知干扰和其运动学模型的不确定性。

本步骤中，在现有技术中通过全向移动机器人的标称运动学模型可以得到轨迹跟踪误差模型。全向移动机器人的跟踪问题也可以是转化为轨迹跟踪误差模型的稳定问题。然而，轨迹跟踪误差模型是基于标称运动学模型而构建，而标称运动学模型并未考虑到存在外部干扰和模型不确定性。因此，本步骤中考虑了集总扰动，并构建具有集总扰动的实际运动学模型。之后，结合标称运动学模型和实际运动学模型构建轨迹跟踪误差模型。轨迹跟踪误差模型考虑了集总误差，进而使得轨迹跟踪误差模型的稳定问题能够准确地映射全向移动机器人的跟踪问题。

S2，针对OMR的轨迹跟踪问题，建立轨迹跟踪误差模型，并在此基础上设计模型预测控制器。

本步骤中，模型预测控制的过程可以看成是找寻代价函数最优解的过程。在该搜索空间的每个采样实例处，N个未来采样实例的输出可从系统模型获得，而代价函数是通过预测和真实系统状态输出之间的估计误差得到的。因此，通过最小化损失函数，导出即时N个采样实例的最优控制输入。值得强调的是，只有第一控制向量被当作系统的输入，这个过程是迭代执行的，并且在每次迭代中实现优化，直到获得最优解。

S3，给定参考机器人的姿态和速度，通过标称运动学模型计算得到参考机器人的位置信息，通过实际运动学模型得到跟随机器人的位置信息；

S4，根据参考机器人的位置信息和跟随机器人的位置信息，结合轨迹跟踪误差模型，计算得到第k时刻跟随机器人的跟踪误差；

S5，通过模型预测控制器获得跟随OMR的最优输入，并根据最优输入和跟踪误差，结合改进滑模观测器，获得跟随OMR的真实集总扰动向量的估计值，根据估计值对跟随OMR进行扰动补偿，进而控制跟随OMR的运动；

S6，循环S3至S6。

由于全向移动机器人具有紧密的机械结构，但大多数控制系统中采用的动力学与运动学模型仅使用标称运动学模型，忽略了机械误差导致的模型不确定性。因此，本申请考虑了全向移动机器人的扰动和不确定性，设计了两种运动学模型：标称运动学模型和实际运动学模型，来映射全向移动机器人的运动状况，进而提高模型预测控制的准确率。参照图1至图4所示，图1为本申请实施例提供的标称运动学模型的示意图,图2为本申请实施例提供的全向移动机器人的结构示意图,图3所示为标称运动学模型的参数定义图。图4所示为实际运动学模型的参数定义图。下面将对标称运动学模型、实际运动学模型和轨迹跟踪误差模型的构建过程进行进一步地说明和阐述。S1可以包括但不限于以下步骤。

S11，建立标称运动学模型。

S111，定义XOY表示惯性坐标系，x_qo_qy_q表示机器人坐标系，并对全向移动机器人的结构进行描述。具体地，定义以全向移动机器人为中心的两个车轮之间的距离表示L和l，定义全向移动机器人的车轮半径为r。可以理解的是，L表示车轮到质心的纵向距离，l表示车轮到质心的横向距离。

S112，对全向移动机器人的速度约束进行描述，获得全向移动机器人的四个麦克纳姆轮的角速度与车身速度之间的关系。

其中，全向移动机器人的车身速度和车轮的角速度的关系可以计算为公式(1)：

公式(1)中：全向移动机器人的四个麦克纳姆轮的速度通过[ω₁ ω₂ ω₃ ω₄]^T表示，全向移动机器人沿x_q、y_q的速度和绕其车身几何中心的旋转角速度通过[v_x′ v_y′ ω′]^T表示。J_r表示雅克比矩阵，具体为：

S113，通过公式(1)，得到如下公式(2)所示的全向移动机器人的标称运动学模型：

公式(2)中：全向移动机器人在惯性坐标系中的位置和方向角通过

表示。

S12，建立实际运动学模型。其过程为：

若要实现全向移动机器人预期的控制效果，需要考虑全向移动机器人在运动时受到的未知干扰和其运动模型的不确定性。本申请将未知干扰和其运动模型的不确定性统一为集总扰动。为了推导全向移动机器人在存在集总扰动的情况下的运动学模型，需要作如下假设：全向移动机器人系统中的未知扰动和模型不确定性都是有界的，其一阶导数也是有界的。

那么，在考虑未知扰动和模型不确定性的情况下，全向移动机器人的车身速度和其四个车轮的角速度的关系为：

公式(3)中：[ω₁ ω₂ ω₃ ω₄]^T表示全向移动机器人的四个麦克纳姆轮的车轮角速度。[v_x v_y ω]^T则表示全向移动机器人沿x_q、y_q的速度和绕其车身几何中心的旋转角速度；全向移动机器人的质心在不同速度方向上的集总扰动通过[f_x f_y f_ω]^T表示，集总扰动包括未知扰动和模型不确定性。

则在存在集总扰动的情况下全向移动机器人的运动学模型满足公式(4)：

公式(4)中：考虑全向移动机器人的集总扰动，全向移动机器人在惯性坐标系中的位置和方向角通过

表示。

S13，构建轨迹跟踪误差模型。

S131，确定参考机器人和跟随机器人的物理参数。

参考机器人描述为：

其中，[v_xr v_yr ω_r]^T表示参考机器人的姿态和速度，

表示参考机器人的位置信息。

跟随机器人描述为：

其中，[ω₁ ω₂ ω₃ ω₄]^T表示全向移动机器人的四个麦克纳姆轮的车轮角速度。[v_x v_y ω]^T则表示全向移动机器人沿x_q、y_q的速度和绕其车身几何中心的旋转角速度；全向移动机器人的质心在不同速度方向上的集总扰动通过[f_x f_y f_ω]^T表示，集总扰动包括未知扰动和模型不确定性。

S132，根据参考机器人和跟随机器人在惯性坐标系下的位置信息，计算跟随机器人在机器人坐标系下的位置和角度的误差：

公式(5)中：

和

分别对应于参考机器人和跟随机器人在机器人坐标系和惯性坐标系下的位置和方向角。

S133，分别计算公式(5)中的_ex、e_y和

的导数，即对公式(5)微分：

S134，将公式(6)的系统在平衡点附近近似线性化，得到轨迹跟踪误差模型。在本申请中，全向移动机器人的跟踪问题可以转化为轨迹跟踪误差模型的稳定问题。

需要说明的是，轨迹跟踪误差模型满足下述公式(7)：

其中，

u₃＝ω_r-ω。

需要说明的是，在轨迹跟踪误差模型中，控制输入[u₁，u₂，u₃],使得误差状态量

可以收敛到原点。

本申请的S1建立了一种新的考虑机械误差及外界扰动的非参数化模型的运动学模型，不仅考虑到传统的误差模型是基于全向移动机器人的标称运动学模型，并且考虑到未知干扰与模型的不确定性。本申请在此标称运动学模型的基础上做出改进，建立了考虑机械误差与外界扰动的全向移动机器人的实际运动学模型，把全向移动机器人的跟踪问题转化为模型的稳定问题。这样保证系统在控制全向移动机器人速度保持在设定值的同时，考虑到未知干扰带来的影响并实现对扰动的补偿，从而达到预期的控制效果。

基于上述实施例，本申请的模型预测控制的本质是一种滚动优化算法。在每个采样时刻，模型预测控制需要在线迭代求解优化问题，以获得控制序列。本申请中，利用公式(2)所示的标称运动学模型来设计模型预测控制器。本申请的一个实施例，下面将对设计模型预测控制器的过程进行进一步地说明和阐述。S2可以包括但不限于以下步骤。

S21，将公式(7)的轨迹跟踪误差模型离散化表示。具体地，根据采样定理，以采样时间T离散化所述轨迹跟踪误差模型，得到如下离散空间模型：

x_k+1＝G_kx_k+H_ku_k (8)

公式(8)中：

输入量通过u_k表示，状态量通过x_k表示。

S22，将N_p和N_c分别定义为离散空间模型的预测水平和控制水平，定义如下代价函数：

其中，Δu_k+j|k＝u_k+j|k-u_k+j-1|k。

公式(9)中：Q和P分别表示相应维数的正定矩阵，x_k+j|k表示第k+j时刻的预测状态，Δu_k+j|k表示输入增量，u_k+j|k表示第k+j时刻的控制输入向量。

S23，定义如下形式的预测向量，即预测序列：

受以下约束：

在约束中：

和

表示状态量

的最小值和最大值。

和

表示输入量

的最小值和最大值，

和

表示输入增量

的最小值和最大值。约束中的最大值和最小值均表示一设定的常数，本申请对此不作具体限定。

那么，对未来时刻状态的预测可以计算为：

上述公式(11)可以改写为以下的紧凑形式：

其中，

符号

表示向左累乘；

S24，将公式(9)表示的代价函数转换为以下优化问题：

约束表示如下：

公式(13)和其约束中：

是一个单位矩阵。而且有

则上述优化问题可以转换为优化约束QP问题：

约束表示如下：

公式(14)中：

基于上述内容，基于约束条件下代价函数的模型预测控制算法可通过求解以上约束QP问题进行优化。

本申请的一个实施例，本申请的S3中，给定参考机器人的姿态和速度[v_xr v_yr ω_r]^T，根据公式(2)的标称运动学模型，可以计算得到参考机器人的位置信息

如图5所示为参考机器人的物理参数定义图。另外，通过公式(4)的实际运动学模型，可以计算得到跟随机器人的位置信息

跟随机器人的物理参数定义与图4所示的实际运动学模型的参数定义相同。

需要说明的是，参考机器人的位置信息满足以下公式：

跟随机器人的位置信息满足以下公式：

本申请的一个实施例，本申请的S4中，通过参考机器人的位置信息

以及跟随机器人的位置信息

通过公式(7)的轨迹跟踪误差模型，获得在第k时刻下跟随机器人的跟踪误差。

基于上述内容，本申请设计了一种改进的VPSMO算法来逼近全向移动机器人运动学模型中的集总扰动，并利用扰动估计消除了扰动对系统的影响，提高了全向移动机器人系统的鲁棒性。本申请的一个实施例，下面将对搭建改进滑模观测器的步骤进行说明和阐述。搭建改进滑模观测器的步骤可以包括但不限于以下步骤。

首先，公式(4)的实际运动学模型可以重新排列为下述公式(15)所示的形式，即将实际运动学模型写成如下状态空间方程：

公式(15)中：真实集总扰动向量表示为f，且f＝[f_x，f_y，f_w]。跟随机器人在x方向上的真实集总扰动向量表示为f_x，在y方向上的真实集总扰动向量表示为f_y，在z方向上的真实集总扰动向量表示为f_w。

并且，

v＝[ω₁ ω₂ ω₃ ω₄]^T，

然后，通过公式(15)的状态空间方程，设计得到滑动面以及变幂次趋近律。

需要说明的是，滑动面被设计为：

公式(16)中：跟随机器人的位置通过s＝[s₁，s₂，s₃]^T来表示，其方向角的估计值通过

来表示。

需要说明的是，变幂次趋近律被设计为：

其中，λ₁和λ₂表示对应的变幂次矩阵。λ₁满足λ₁＝diag(λ₁₁，λ₁₂，λ₁₃)；λ₂满足

λ₂＝diag(λ₂₁，λ₂₂，λ₂₃)。K_i＝diag(k_i1，k_i2，k_i3)，i＝1，2，3，4表示合适的正定矩阵。

其中：

饱和函数sat(si)，i＝1，2，3，4可以被设计为：

χ＞1表示边界厚度。

本实施例中，通过边界层设计，引入饱和函数代替传统的符号函数，消除观测器输出的抖振。

此外，对于公式(17)，还有：

其中，η₁＞1，0＜η₂＜1，η₃＞1，σ是一个趋于无穷的常数。

上述变幂次趋近律变幂次趋近律结合了双幂趋近律和指数趋近律的优点。变幂次趋近律能适应滑动变量s的变化，并可自适应调整，从而保证整个过程的快速收敛速度。

具体地，定义变量s_σ＝[s_1σ，s_2σ，s_3σ]^T，其中s_iσ＝s_i-χsat(s_i)。需要说明的是，变量_sσ满足

和

这两个条件。在本申请中，

为引理1，

为引理2。新的趋近律，即公式(17)的变幂次趋近律，无论滑动面s是否接近零，都能显著地提高滑模观测器的收敛速度。当|s_i|≥1时，λ_1i＝η₁|s_i|+η₂，λ_2i＝η₃均对加速滑模观测器的收敛有重要的作用。当|s_i|＜1且λ_1i＝η₂，λ_2i＝1时，改进滑模观测器的趋近率等于快速幂次趋近率。

因此，通过以上公式，可以构建得到改进滑模观测器。改进滑模观测器被设计为：

通过上述两个引理，可以得到本申请的定理：对于公式(15)对应的状态空间方程，本申请提供的改进滑模观测器可以准确地估计全向移动机器人的集总扰动，通过扰动补偿可以使得全向移动机器人的闭环系统更为稳定。证明如下：

选择下列Lyapunov函数:

显然，函数V是正定的。定义d_dis＝Df＝[d_1dis，d_2dis，d_3dis]对Lyapunov函数进行求导，得到如下公式：

前述在描述构建实际运动学模型时，提出过假设：全向移动机器人系统中的未知扰动和模型不确定性都是有界的，其一阶导数也是有界的。结合上述假设，可以推导出变量d_dis是有界的，因此可以使矩阵K₂满足|d_idis|＜k_2i，i＝1，2，3。在这种情况下，容易证明

即变量s_σ可以收敛于零。滑动面s收敛到大小可以设计的边界层内部。至此，定理证明结束。

基于上述内容可以得知，本申请设计的改进的变幂次趋近率可以彻底消除系统中的抖动，使得整个系统收敛速度更快、误差更小。为了提高观测器的性能，与传统的滑模观测器相比，本申请的改进滑模观测器(VPSMO)的变幂次趋近律结合了双幂趋近律和指数趋近律的优点，能够适应滑动面变量s的变化，并可自适应调整，从而保证整个过程的快速收敛速度。并且，在观测器的设计中，本申请通过边界层设计引入饱和函数代替传统的符号函数，消除观测器输出的抖振。如图6所示，图6中展示了本申请的改进滑模观测器输出的真实集总扰动向量的估计值和真实集总扰动向量的仿真图，其横坐标表示时间，单位是秒。通过图6可以得知，本申请所设计的改进滑模观测器可以准确地估计真实集总扰动向量f的值，因此扰动估计误差

满足：

本申请提出的VPSMO-MPC算法可以有效地消除集总扰动f的影响，实现快速稳定的跟踪控制。

基于上述内容，本申请的S5可以看做是VPSMO和MPC算法的结合并应用于全向移动机器人上。本申请的一个实施例，下面对S5进行说明和阐述。S5可以包括但不限于以下步骤。

S51，通过模型预测控制器，即公式(14)的约束QP问题，得到最优控制输入增量

求解得到最优输入

S52，根据最优输入

计算第k时刻跟随机器人的车身速度；

具体地，k时刻跟随机器人的车身速度可以计算为：

0(k)＝ω_r(k)-u₃(k)

S53，通过公式(18)的改进滑模观测器，获得跟随机器人的真实集总扰动向量f的估计值

需要说明的是，真实集总扰动向量的估计值

表示跟随机器人在x方向的真实集总扰动向量的估计值；

表示跟随机器人在y方向的真实集总扰动向量的估计值；

表示跟随机器人在z方向上的真实集总扰动向量的估计值。

S531，将公式(15)的状态空间方程减去公式(18)的改进滑模观测器，得到如下观测误差：

S532，当观测误差到达滑动面，那么有：

S533，结合公式(19)和公式(20)，则得到真实集总扰动向量f的估计值

其中：

本步骤中，改进滑模观测器可以准确地估计真实集总扰动向量f。

S54，根据估计值

对k时刻跟随机器人的车身速度进行扰动估计补偿，计算得到作用在跟随机器人的四个麦克纳姆轮的电机上的实际角速度。

其中，实际角速度满足公式(22)，公式(22)实际为逆运动学公式：

公式(22)中：

表示雅克比矩阵J_r的广义逆矩阵。ω₁(k)、ω₂(k)、ω₃(k)、ω₄(k)分别表示第k时刻跟随机器人的四个麦克纳姆轮的车轮角速度；

表示第k时刻跟随机器人在x方向的真实集总扰动向量的估计值，

表示第k时刻跟随机器人在y方向上的真实集总扰动向量的估计值，

表示第k时刻跟随机器人在z方向上的真实集总扰动向量的估计值。

S55，结合公式(22)的实际角速度和第k时刻进行扰动补偿后跟随机器人的实际运动学模型，得到进行扰动补偿后的实际运动学模型。

其中，第k时刻进行扰动补偿后跟随机器人的实际运动学模型通过公式(4)的实际运动学模型而得，其满足下述公式(23)：

公式(23)中：

并且，f_x(k)表示第k时刻跟随机器人在x方向上的真实集总扰动向量；f_y(k)表示第k时刻跟随机器人在y方向上的真实集总扰动向量；f_ω(k)表示第k时刻跟随机器人在z方向上的真实集总扰动向量。

那么，进行扰动补偿后的实际运动学模型满足公式(24)：

公式(24)中：跟随机器人在x、y、z三个方向上的扰动估计误差通过

表示，扰动估计误差为真实集总扰动向量与其估计值之差。

S56，根据进行扰动补偿后的实际运动学模型，生成与作用在跟随OMR的四个麦克纳姆轮的电机上的实际角速度匹配的速度指令并输出给跟随OMR，控制跟随OMR的速度，跟随OMR更新其位置信息。

基于上述内容，本申请提出了一种结合MPC和VPSMO的复合控制算法，用于未知扰动及模型参数摄动下的全向移动机器人的轨迹跟踪控制。如图7所示，图7为本申请实施例提供的跟随机器人和参考机器人的轨迹跟踪结果图。图7中展示了通过MPC算法控制的全向移动机器人的跟踪轨迹、通过VPSMO-MPC算法控制的全向移动机器人的跟踪轨迹以及参考轨迹。让物理属性完全相同的两个全向移动机器人分别用两种算法跟踪参考轨迹，两种算法分别为传统的MPC算法和本申请提供的VPSMO-MPC算法。传统的MPC算法需要精确的模型数据。当全向移动机器人的运动学模型不准确时，MPC可能无法达到预期效果。基于此，本申请创新性地改进了SMO并将其与MPC结合，实现了同时处理物理约束与未知干扰影响的全向移动机器人的控制系统，大大提高其鲁棒性与跟踪性能。如图8所示，图8为本申请实施例提供的现有的MPC算法和本申请的VPSMO-MPC算法的矩形轨迹跟踪误差比较图。通过图8可知：本申请的VPSMO-MPC相较于传统MPC更为优秀的跟踪能力。

参照图9所示，图9所示为本申请实施例提供的OMR轨迹跟踪控制方法的控制原理图。控制过程为：

考虑一台如图2所示的全向移动机器人。全向移动机器人均装有四个麦克纳姆轮，轮子沿着全向移动机器人的平台两侧成对安装，且相对于其质心是均匀的。全向移动机器人可以沿着x、y轴平移以及绕着z轴旋转。定义两个全向移动机器人的坐标系：惯性坐标系XOY、机器人坐标系x_qo_qy_q以及定义机器人的参数。可以根据已知参数代入已建立的标称运动学模型和实际运动学模型中。在两个运动学模型的基础上建立公式(7)的轨迹跟踪误差模型。在轨迹跟踪误差模型中，控制输入[u₁ u₂ u₃],使得误差状态量

可以收敛到原点。另外，需要提前构建模型预测控制器和改进滑模观测器。基于约束条件下代价函数的模型预测控制算法可通过解约束来对公式(14)所示的优化约束QP问题进行优化：

约束表示如下：

公式(14)中：

为了验证所提出的VPSMO-MPC算法，使用一台参考机器人和一台跟随机器人进行轨迹跟踪控制，参考机器人的参数如图5所述，跟随机器人的参数与实际运动学模型相同。其中，参考机器人的运动学模型遵循标称运动学模型，其轨迹随着时间变化。跟随机器人的运动考虑集总扰动，其运动学模型遵循实际运动学模型，其轨迹随着参考机器人的轨迹行走。

首先，根据参考机器人的姿态和速度[v_xr v_yr ω_r]^T，通过公式(2)的标称运动学模型可以得到参考机器人的位置信息

参考机器人的主要参数如图4所示。而由于跟随机器人的运动考虑集总扰动，因此其位置信息需要通过公式(4)的实际运动学模型而获得。跟随机器人的位置信息为

通过公式(7)的轨迹跟踪误差模型得到第k时刻跟随机器人的跟踪误差。通过模型预测控制器可以获得最优输入

然后，通过最优输入反解出k时刻跟随机器人的车身速度v_x(k)、v_y(k)、ω(k)。

之后，通过跟随机器人当前四个车轮的角速度和其位置信息

通过改进滑模观测器，得到真实集总扰动向量f的估计值

公式(22)表示逆运动学公式，对应于作用在跟随机器人的四个麦克纳姆轮的电机上的实际角速度。在公式(22)中加入扰动估计补偿，从而得到作用在跟随机器人的四个麦克纳姆轮的电机上的实际角速度。需要说明的是，公式(22)与理论的逆运动学公式不同，理论的逆运动学公式并无扰动补偿。

最终，将上一步骤获得的实际角速度对应的速度指令作用于跟随机器人，跟随机器人更新自身的位置信息。重复上述步骤，校准跟随机器人的跟踪轨迹。

本发明提出一种考虑未知扰动和模型不确定性的全向移动机器人的运动学模型，在现有的标称运动学模型的基础上，加入了未知扰动和模型不确定性的实际运动学模型，使得全向移动机器人的运动学模型更加完善。实际运动学模型能与改进滑模控制器更有效地结合，对未知扰动进行逼近，从而实现扰动补偿。与传统的SMO相比，本申请的控制方法大大提高了系统的容错率和鲁棒性，消除了抖振的影响，从而提高收敛速度；并且，消除了未知干扰和不确定性的影响，实现更精确的控制，降低了系统的误差。

另一方面，本申请提供了一种存储介质，其中存储有处理器可执行的指令，所述处理器可执行的指令在由处理器执行时用于执行所述的基于复合控制算法的OMR轨迹跟踪控制方法。

本申请的说明书及上述附图中的术语“第一”、“第二”、“第三”、“第四”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例例如能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或装置不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或装置固有的其他步骤或单元。

应当理解，在本申请中，“至少一个(项)”是指一个或者多个，“多个”是指两个或两个以上。“和/或”，用于描述关联对象的关联关系，为可以存在三种关系，例如，“A和/或B”可以为：只存在A，只存在B以及同时存在A和B三种情况，其中A，B可以是单数或者复数。字符“/”一般为前后关联对象是一种“或”的关系。“以下至少一项(个)”或其类似表达，是指这些项中的任意组合，包括单项(个)或复数项(个)的任意组合。例如，a，b或c中的至少一项(个)，可以为：a，b，c，“a和b”，“a和c”，“b和c”，或“a和b和c”，其中a，b，c可以是单个，也可以是多个。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统、装置和方法，可以通过其他的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口、装置或单元的间接耦合或通信连接，可以是电性、机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机装置(可以是个人计算机、服务器或者网络装置等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：只读存储器(Read-On ly Memory，简称ROM)、随机存取存储器(Random Access Memory，简称RAM)、磁碟或者光盘等各种可以存储程序代码的介质。

对于上述方法实施例中的步骤编号，其仅为了便于阐述说明而设置，对步骤之间的顺序不做任何限定，实施例中的各步骤的执行顺序均可根据本领域技术人员的理解来进行适应性调整。

Claims (10)

1.基于复合控制算法的OMR轨迹跟踪控制方法,其特征在于,包括如下步骤:

S1，建立标称运动学模型和考虑集总扰动的实际运动学模型，并构建轨迹跟踪误差模型，所述集总扰动包括OMR在运动时的未知干扰和其运动学模型的不确定性；

S2，针对OMR的轨迹跟踪问题，建立了轨迹跟踪误差模型，并在此基础上设计模型预测控制器；

S3，给定参考OMR的姿态和速度，通过标称运动学模型计算得到参考OMR的位置信息，通过实际运动学模型得到跟随OMR的位置信息；

S4，根据参考OMR的位置信息和跟随OMR的位置信息，结合轨迹跟踪误差模型，计算得到第k时刻跟随OMR的跟踪误差；

S5，通过模型预测控制器获得跟随OMR的最优输入，并根据最优输入和跟踪误差，结合改进滑模观测器，获得跟随OMR的真实集总扰动向量的估计值，根据估计值对跟随OMR进行扰动补偿，进而控制跟随OMR的运动；

S6，循环S3至S5。

2.根据权利要求1所述的基于复合控制算法的OMR轨迹跟踪控制方法，其特征在于，S1中建立标称运动学模型的步骤包括：

构建惯性坐标系XOY和机器人坐标系x_qo_qy_q，对OMR的结构进行描述；

对OMR的速度约束进行描述，获得OMR的四个麦克纳姆轮的角速度与车身速度之间的关系如下：

其中，[ω₁ ω₂ ω₃ ω₄]^T为OMR的四个麦克纳姆轮的角速度，[v_x′ v_y′ ω′]^T为OMR沿x_q、y_q的速度和绕其车身几何中心的旋转角速度，J_r为如下雅克比矩阵：

其中，r为麦克纳姆轮的半径，L和l分别为麦克纳姆轮到其质心的纵向距离和横向距离；

通过上述公式，得出所述标称运动学模型如下：

其中，

为OMR在惯性坐标系中的位置和方向角。

3.根据权利要求2所述的基于复合控制算法的OMR轨迹跟踪控制方法，其特征在于，S1中建立考虑集总扰动的实际运动学模型的步骤包括：

考虑OMR的集总扰动，计算得到如下OMR的四个麦克纳姆轮的角速度与车身速度之间的关系：

其中，[ω₁ ω₂ ω₃ ω₄]^T为OMR的四个麦克纳姆轮的车轮角速度；[v_x v_y ω]^T为OMR沿x_q、y_q的速度和绕其车身几何中心的旋转角速度；J_r为雅克比矩阵；[f_x f_y f_ω]^T为OMR的质心在不同速度方向上的集总扰动；

通过上述公式，得到所述实际运动学模型如下：

其中，

为考虑OMR的集总扰动，OMR在惯性坐标系中的位置和方向角。

4.根据权利要求1所述的基于复合控制算法的OMR轨迹跟踪控制方法，其特征在于，S1中构建轨迹跟踪误差模型的步骤包括：

确定参考OMR和跟随OMR的物理参数；对于参考OMR，有：

其中，[v_xr v_yr ω_r]^T为参考OMR的速度，

为参考OMR的位置信息；

对于跟随OMR，有：

其中，[ω₁ ω₂ ω₃ ω₄]^T为OMR的四个麦克纳姆轮的车轮角速度；[v_x v_y ω]^T为OMR沿x_q、y_q的速度和绕其车身几何中心的旋转角速度；J_r为雅克比矩阵；[f_x f_y f_ω]^T为OMR的质心在不同速度方向上的集总扰动；

根据参考OMR和跟随OMR在惯性坐标系下的位置信息，计算跟随OMR在机器人坐标系下的位置和角度的误差：

其中，

以及

分别为参考OMR和跟随OMR在机器人坐标系和惯性坐标系下的位置和方向角；

根据所得到的参考OMR在机器人坐标系下的相对位置和角度，分别计算e_x、e_y和

的导数，所述e_x、e_y和

的导数分别为：

则综上轨迹跟踪误差模型为：

其中：

u₃＝ω_r-ω。

5.根据权利要求1所述的基于复合控制算法的OMR轨迹跟踪控制方法，其特征在于，S2中，针对OMR的轨迹跟踪问题和轨迹跟踪误差模型，设计模型预测控制器，包括：

S21，根据采样定理，以采样时间T离散化所述轨迹跟踪误差模型，得到如下离散空间模型：

x_k+1＝G_kx_k+H_ku_k

其中:

u_k为输入量，x_k为状态量，T为采样时间；

S22，定义代价函数为：

其中，Δu_k+j|k＝u_k+j|k-u_k+j-1|k；

其中，Q和P分别为相应维数的正定矩阵，x_k+j|k为第k+j时刻的预测状态，Δu_k+j|k为输入增量，u_k+j|k为第k+j时刻的控制输入向量，N_p为离散空间模型的预测水平，N_c为离散空间模型的控制水平；

S23，定义如下形式的预测向量：

约束为：

其中，状态量

的最小值

和最大值

输入量

的最小值

和最大值

输入增量

的最小值

和最大值

均为常数；

则未来时刻状态的预测为：

其中：

符号

为向左累乘；

S24，将代价函数转化为如下优化问题：

受以下约束：

其中：

是一个单位矩阵，并且

最终，得到优化约束QP问题：

受

约束；

其中：

基于约束条件下代价函数的模型预测控制算法通过求解以上约束QP问题进行优化。

6.根据权利要求1所述的基于复合控制算法的OMR轨迹跟踪控制方法，其特征在于，S3具体包括：

给定参考OMR的姿态和速度[v_xr v_yr ω_r]^T，通过标称运动学模型计算得到参考OMR的位置信息

其中，参考OMR的位置信息为：

通过实际运动学模型得到跟随OMR的位置信息；其中跟随OMR的位置信息为：

其中，[ω₁ ω₂ ω₃ ω₄]^T为OMR的四个麦克纳姆轮的车轮角速度；[v_x v_y ω]^T为OMR沿x_q、y_q的速度和绕其车身几何中心的旋转角速度；J_r为雅克比矩阵；[f_x f_y f_ω]^T为OMR的质心在不同速度方向上的集总扰动。

7.根据权利要求1所述的基于复合控制算法的OMR轨迹跟踪控制方法，其特征在于，所述改进滑模观测器的构建步骤包括：

将所述实际运动学模型写成状态空间方程，状态空间方程可以表示为：

其中：f为真实集总扰动向量，f＝[f_x，f_y，f_w]；f_x、f_y和f_w分别为跟随OMR在x方向、y方向和z方向上的真实集总扰动向量；并且，有：

v＝[ω₁ ω₂ ω₃ ω₄]^T，

通过上述状态空间方程，设计得到滑动面和变幂次趋近律；

所述滑动面为

其中：s＝[s₁，s₂，s₃]^T和

分别为跟随OMR的位置和方向角的估计值；

所述变幂次趋近律为：

其中：

sat(s_i)为饱和函数；λ₁和λ₂为对应的变幂次矩阵，有λ₁＝diag(λ₁₁，λ₁₂，λ₁₃)且λ₂＝diag(λ₂₁，λ₂₂，λ₂₃)；K_i＝diag(k_i1，k_i2，k_i3)，i＝1，2，3，4为正定矩阵；所述饱和函数为：

χ＞1为边界厚度；

通过以上公式，构建如下改进滑模观测器：

其中，当|s_i|≥1时，λ_1i＝η₁|s_i|+η₂，λ_2i＝η₃加速所述改进滑模观测器的收敛；当|s_i|＜1且λ_1i＝η₂，λ_2i＝1时，所述改进滑模观测器的趋近率等于快速幂次趋近率。

8.根据权利要求7所述的基于复合控制算法的OMR轨迹跟踪控制方法，其特征在于，S5具体包括：

S51，通过模型预测控制器，得到最优控制输入增量，求解得到最优输入

S52，根据最优输入

计算第k时刻跟随OMR的车身速度：

ω(k)＝ω_r(k)-u₃(k)

S53，通过改进滑模观测器得到跟随OMR的真实集总扰动向量f的估计值

所述真实集总扰动向量的估计值

满足

其中，

和

为跟随OMR在x方向、y方向和z方向上的真实集总扰动向量的估计值，且

S54，根据所述估计值，对第k时刻跟随OMR的车身速度进行扰动估计补偿，计算得到作用在跟随OMR的四个麦克纳姆轮的电机上的实际角速度：

其中，ω₁(k)、ω₂(k)、ω₃(k)、ω₄(k)分别为第k时刻跟随OMR的四个麦克纳姆轮的车轮角速度；

分别为第k时刻跟随OMR在x方向、y方向和z方向上的真实集总扰动向量的估计值；

其中，

为雅克比矩阵的广义逆矩阵；

S55，结合实际角速度和第k时刻进行扰动补偿后跟随OMR的实际运动学模型，得到进行扰动补偿后的实际运动学模型；

其中，第k时刻进行扰动补偿后跟随OMR的实际运动学模型为：

其中：

f_x(k)、f_y(k)、f_ω(k)分别为第k时刻跟随OMR在x、y、z方向上的真实集总扰动向量；J_r为雅克比矩阵；

所述进行扰动补偿后的实际运动学模型为：

其中：

为跟随OMR在x、y、z方向上的真实集总扰动向量与其估计值的差值，即扰动估计误差；

S56，根据进行扰动补偿后的实际运动学模型，生成与作用在跟随OMR的四个麦克纳姆轮的电机上的实际角速度匹配的速度指令并输出给跟随OMR，控制跟随OMR的速度，跟随OMR更新其位置信息。

9.根据权利要求8所述的基于复合控制算法的OMR轨迹跟踪控制方法，其特征在于，所述通过改进滑模观测器得到跟随OMR的真实集总扰动向量f的估计值，包括：

将状态空间方程和所述改进滑模观测器进行作差运算，得到观测误差；所述观测误差为：

当观测误差到达滑动面，则存在有如下关系：

最终，结合观测误差和

得到如下真实集总扰动向量f的估计值：

10.一种存储介质，其中存储有处理器可执行的指令，其特征在于，所述处理器可执行的指令在由处理器执行时用于执行如权利要求1-9任一项所述的基于复合控制算法的OMR轨迹跟踪控制方法。

Images