CN109828528A - 机器人轨迹跟踪方法及装置 - Google Patents

Abstract

本发明提供了一种机器人轨迹跟踪方法及装置，该方法应用于双闭环滑模控制器，所述方法包括：基于快速幂次趋近律确定机器人的横轴位置跟踪控制律和纵轴位置跟踪控制律；根据所述横轴位置跟踪控制律和所述纵轴位置跟踪控制律确定机器人的线速度控制律；基于饱和函数指数趋近律确定机器人的姿态控制律；根据所述线速度控制律和所述姿态控制律对机器人轨迹进行跟踪。本发明提供的机器人轨迹跟踪方法及装置能够有效保障机器人闭环系统的稳定性。

Description

机器人轨迹跟踪方法及装置

技术领域

本发明属于智能控制技术领域，更具体地说，是涉及一种机器人轨迹跟踪方法及装置。

背景技术

机器人的控制系统涉及到伺服驱动、运动控制、计算机软件等，机器人轨迹跟踪控制是指通过给定各关节的驱动力矩，使机器人的位置、速度等状态变量跟踪给定的理想轨迹，对于整个轨迹来说，都需要严格控制，是工业生产中应用最为广泛的控制方式。但是由于机器人系统是一个复杂的多输入多输出的非线性系统，具有时变、强耦合和非线性的动力学特性，因此，轨迹跟踪控制十分复杂与困难。研究机器人轨迹跟踪控制以及提高轨迹跟踪控制的精度对机器人技术有着重要的意义。

现有技术中，当机器人检测到目标位置改变后，采用先转后走的控制策略，即先控制机器人转向使机器人与目标方向一致，然后控制机器人快速跟踪目标轨迹。外环控制器通过调整机器人的线速度使之快速跟随目标，其在目标已经完成转向之后进行。故在位置控制器中假定机器人的方向与目标方向一致。然而在实际跟踪的初始阶段机器人的方向无法做到与目标方向一致造成闭环系统不稳定。

发明内容

本发明的目的在于提供一种机器人轨迹跟踪方法及装置，以解决现有技术中存在的机器人闭环系统不稳定的技术问题。

本发明实施例的第一方面，提供了一种机器人轨迹跟踪方法，包括：

基于快速幂次趋近律确定机器人的横轴位置跟踪控制律和纵轴位置跟踪控制律；

根据所述横轴位置跟踪控制律和所述纵轴位置跟踪控制律确定机器人的线速度控制律；

基于饱和函数指数趋近律确定机器人的姿态控制律；

根据所述线速度控制律和所述姿态控制律对机器人轨迹进行跟踪。

本发明实施例的第二方面，提供了一种机器人轨迹跟踪装置，包括：

横纵轴控制律确定模块，用于基于快速幂次趋近律确定机器人的横轴位置跟踪控制律和纵轴位置跟踪控制律；

线速度控制律确定模块，用于根据所述横轴位置跟踪控制律和所述纵轴位置跟踪控制律确定机器人的线速度控制律；

姿态控制律确定模块，用于基于饱和函数指数趋近律确定机器人的姿态控制律；

轨迹跟踪模块，用于根据所述线速度控制律和所述姿态控制律对机器人轨迹进行跟踪。

本发明实施例的第三方面，提供了一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述的机器人轨迹跟踪方法的步骤。

本发明实施例的第四方面，提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现上述的机器人轨迹跟踪方法的步骤。

本发明提供的机器人轨迹跟踪方法及装置的有益效果在于：与现有技术相比，本发明提供的机器人轨迹跟踪方法及装置在进行线速度控制时，采用快速幂次趋近律解决了使用传统幂次趋近律的跟踪速度问题以及指数趋近律的抖振问题；在进行方向角度控制时，采用基于饱和函数的指数趋近律，保证传统指数趋近律趋近速度的同时降低了抖振。无论是线速度控制还是方向控制均有效地保障了机器人闭环系统的稳定性。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明一实施例提供的移动机器人的目标跟踪示意图；

图2为本发明一实施例提供的双闭环滑模控制器的结构示意图；

图3为本发明一实施例提供的机器人轨迹跟踪算法的流程示意图；

图4为本发明一实施例提供的机器人轨迹跟踪装置的结构框图；

图5为本发明一实施例提供的终端设备的示意框图。

具体实施方式

为了使本发明所要解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

在描述本发明实施例提供的机器人轨迹跟踪方法之前，请参考图1，图1为本发明一实施例提供的移动机器人的目标跟踪示意图。其中，Mobile为移动机器人，Target表示移动机器人的目标位置。在本实施例中，差分非完整约束移动机器人，规定坐标系原点为笛卡尔坐标原点(O,X,Y)。则在笛卡尔坐标系下机器人的位置和角度定义为p＝(x,y,θ)^T，其中(x,y)是机器人的位置，θ为机器人朝向与坐标轴x角度。(x_d,y_d)为目标期望位置。

则机器人的非完整约束为：

其中，为机器人当前位置。轮式移动机器人的线速度在车轮轴线方向为0，则非完整约束机器人的运动学模型为：

其中，为机器人的当前方向角度，输入v和w是移动机器人的线速度和角速度。进一步地，移动机器人的非完整运动学描述为：

本发明实施例提供的机器人轨迹跟踪方法应用于双闭环滑模控制器，在描述本发明实施例提供的机器人轨迹跟踪方法之前，请参考图2，图2为本发明一实施例提供的双闭环滑模控制器的结构示意图。

在机器人轨迹跟踪控制系统中，传感器获得目标位置改变后，得到目标的位置信息(x_d,y_d)。机器人根据自身位置角度信息与目标位置角度信息的关系，采用机器人先转后走的控制策略跟随目标轨迹。因此可采用如图2所示的双闭环滑模控制器，在内环，构造角度控制器控制机器人的角速度以保持机器人和目标位置在同一方向上，外环构造位置控制器调节机器人的线速度以快速跟随目标。

请参考图3，图3为本发明一实施例提供的机器人轨迹跟踪方法的流程示意图。该方法包括：

S101：基于快速幂次趋近律确定机器人的横轴位置跟踪控制律和纵轴位置跟踪控制律。

在本实施例中，对于横纵坐标的跟踪采用积分滑模面的方式。在基于积分滑模面进行跟踪时，传统幂次趋近律在到达滑模面时趋近速度为0，从而能平滑地进入滑动模态，消除抖振，然而其在远离滑模面时趋近速度过小，降低了跟踪速度。指数趋近律虽能加快远离滑模面时的趋近速度，但在进入滑模面时会产生上下穿越的抖振问题。因此可采用快速幂次趋近律确定横纵轴位置跟踪控制律。

S102：根据横轴位置跟踪控制律和纵轴位置跟踪控制律确定机器人的线速度控制律。

在本实施例中，在机器人双闭环控制系统中，采用先转后走的控制策略。当检测到目标位置改变后，机器人先转向，保持与目标方向一致，再以合适的线速度跟踪目标。因此位置控制器是在机器人与目标方向一致的前提下进行设计的，机器人的线速度控制也可根据横纵轴控制律以及机器人与横纵轴之间的夹角来确定。

S103：基于饱和函数指数趋近律确定机器人的姿态控制律。

在本实施例中，方向控制器作为双环控制器中的内环，用以按照姿态控制律w控制机器人的方向角度θ跟踪理想方向角度θ_d。但在跟踪的初始阶段，实际模型参数θ与理想方向θ_d不一致，会造成闭环系统不稳定。因此，本发明实施例选择收敛速度更快的指数趋近律，使得数θ尽快跟踪θ_d。然而传统的指数趋近律在远离滑模面时具有较快的趋近速度，在靠近滑模面时，由于含有常数项而产生较大的抖振。为保证指数趋近律的趋近速度同时降低抖振，可用饱和函数代替符号函数，即采用基于饱和函数的指数趋近律确定机器人的姿态控制律。

S104：根据线速度控制律和姿态控制律对机器人轨迹进行跟踪。

从上述描述可知，本发明实施例提供的机器人轨迹跟踪方法在进行线速度控制时，采用快速幂次趋近律解决了使用传统幂次趋近律的跟踪速度问题以及指数趋近律的抖振问题；在进行方向角度控制时，采用基于饱和函数的指数趋近律，保证传统指数趋近律趋近速度的同时降低了抖振。无论是线速度控制还是方向控制均有效地保障了机器人闭环系统的稳定性。

可选地，作为本发明实施例提供的机器人轨迹跟踪算法的一种具体实施方式，横轴位置跟踪控制律为：

其中，u₁为横轴位置跟踪控制律，为机器人横轴当前目标位置，k_x为横轴积分时间，k_x＞0保证跟踪误差在理想滑模面上滑动直到系统稳定，x_e为横轴位置跟踪误差，ε_x＞0为横轴第一控制律系数，η_x＞0为横轴第二控制律系数；

其中，s_x为横轴积分滑模面：

在本实施例中，方向控制器的核心是控制机器人的位置坐标(x,y)跟随目标位置(x_d,y_d)。因此，跟踪误差可以为：

积分项具有消除抖振减小稳态误差的作用，对于横轴坐标可采用积分滑模面(5)进行跟踪，并根据得到横轴位置跟踪控制律(4)。

可选地，作为本发明实施例提供的机器人轨迹跟踪算法的一种具体实施方式，纵轴位置跟踪控制律为：

其中，u₂为纵轴位置跟踪控制律，为机器人纵轴当前目标位置，k_y为纵轴积分时间，y_e为纵轴位置跟踪误差，ε_y为纵轴第一控制律系数，η_y为纵轴第二控制律系数；

其中，s_y为横轴积分滑模面：

在本实施例中，根据横轴位置跟踪控制律的确定方法，首先确定纵轴积分滑模面(8)，再根据确定纵轴位置跟踪控制律(7)。

可选地，作为本发明实施例提供的机器人轨迹跟踪算法的一种具体实施方式，线速度控制律的确定方法包括：

其中，θ_d为：

其中，u₁为横轴位置跟踪控制律，u₂为纵轴位置跟踪控制律。

在本实施例中，位置控制器是在机器人与目标方向一致的前提下进行设计，也即θ＝θ_d，因此可根据确定线速度控制律。

可选地，作为本发明提供的机器人轨迹跟踪方法的一种具体实施方式，还包括对线速度控制律和姿态控制律的稳定性分析。

该稳定性分析方法具体可为：

选取Lyapunov函数由Lyapunov稳定性理论可知，当满足时，满足滑模稳定条件。

对于外环位置控制器：

其中，ε_x,η_x为常数，可知满足稳定性条件。

对于内环的方向控制器：

其中，k_θ,η_θ为常数，则可知满足稳定性条件。

对应于上文实施例的机器人轨迹跟踪方法，图4为本发明一实施例提供的机器人轨迹跟踪装置的结构框图。为了便于说明，仅示出了与本发明实施例相关的部分。参考图4，该装置包括：横纵轴控制律确定模块100，线速度控制律确定模块200，姿态控制律确定模块300，轨迹跟踪模块400。

其中，横纵轴控制律确定模块100，用于基于快速幂次趋近律确定机器人的横轴位置跟踪控制律和纵轴位置跟踪控制律。

线速度控制律确定模块200，用于根据横轴位置跟踪控制律和纵轴位置跟踪控制律确定机器人的线速度控制律。

姿态控制律确定模块300，用于基于饱和函数指数趋近律确定机器人的姿态控制律。

轨迹跟踪模块400，用于根据线速度控制律和姿态控制律对机器人轨迹进行跟踪。

可选地，参考图4，作为本发明实施例提供的机器人轨迹跟踪装置的一种具体实施方式，横纵轴控制律确定模块100包括横轴控制律确定单元101，用于确定横轴位置跟踪控制律，横轴位置跟踪控制律的确定方法包括：

其中，u₁为横轴位置跟踪控制律，为机器人横轴当前目标位置，k_x为横轴积分时间，x_e为横轴位置跟踪误差，ε_x为横轴第一控制律系数，η_x为横轴第二控制律系数；

其中，s_x为横轴积分滑模面：

可选地，参考图4，作为本发明实施例提供的机器人轨迹跟踪装置的一种具体实施方式，横纵轴控制律确定模块100包括纵轴控制律确定单元102，纵轴控制律确定单元102用于确定纵轴位置跟踪控制律，纵轴位置跟踪控制律的确定方法包括：

其中，u₂为纵轴位置跟踪控制律，为机器人纵轴当前目标位置，k_y为纵轴积分时间，y_e为纵轴位置跟踪误差，ε_y为纵轴第一控制律系数，η_y为纵轴第二控制律系数；

其中，s_y为横轴积分滑模面：

可选地，作为本发明实施例提供的机器人轨迹跟踪装置的一种具体实施方式，线速度控制律的确定方法包括：

其中，θ_d为：

其中，u₁为横轴位置跟踪控制律，u₂为纵轴位置跟踪控制律。

可选地，作为本发明实施例提供的机器人轨迹跟踪装置的一种具体实施方式，姿态控制律的确定方法包括：

其中，w为姿态控制律，为当前目标方向角度，k_θ为方向积分时间，s_θ为方向积分滑模面，η_θ为方向第一控制系数；

其中，sat(s_θ)为：

其中，φ(s_θ)是为了引进边界层而给出的边界，其在s_θ的邻域。

可选地，作为本发明提供的机器人轨迹跟踪装置的一种具体实施方式，还包括对线速度控制律和姿态控制律的稳定性分析的分析模块。

该分析模块的稳定性分析方法具体可为：

选取Lyapunov函数由Lyapunov稳定性理论可知，当满足时，满足滑模稳定条件。

对于外环位置控制器：

其中，ε_x,η_x为常数，可知满足稳定性条件。

对于内环的方向控制器：

其中，k_θ,η_θ为常数，则可知满足稳定性条件。

参见图5，图5为本发明一实施例提供的一种终端设备的示意框图。如图5所示的本实施例中的终端600可以包括：一个或多个处理器601、一个或多个输入设备602、一个或多个输出设备603及一个或多个存储器604。上述处理器601、输入设备602、则输出设备603及存储器604通过通信总线605完成相互间的通信。存储器604用于存储计算机程序，计算机程序包括程序指令。处理器601用于执行存储器604存储的程序指令。其中，处理器601被配置用于调用程序指令执行以下操作上述各装置实施例中各模块/单元的功能，例如图4所示模块100至400的功能。

应当理解，在本发明实施例中，所称处理器601可以是中央处理单元(CentralProcessing Unit，CPU)，该处理器还可以是其他通用处理器、数字信号处理器(DigitalSignal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

输入设备602可以包括触控板、指纹采传感器(用于采集用户的指纹信息和指纹的方向信息)、麦克风等，输出设备603可以包括显示器(LCD等)、扬声器等。

该存储器604可以包括只读存储器和随机存取存储器，并向处理器601提供指令和数据。存储器604的一部分还可以包括非易失性随机存取存储器。例如，存储器604还可以存储设备类型的信息。

具体实现中，本发明实施例中所描述的处理器601、输入设备602、输出设备603可执行本发明实施例提供的机器人轨迹跟踪方法的第一实施例和第二实施例中所描述的实现方式，也可执行本发明实施例所描述的终端的实现方式，在此不再赘述。

在本发明的另一实施例中提供一种计算机可读存储介质，计算机可读存储介质存储有计算机程序，计算机程序包括程序指令，程序指令被处理器执行时实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，计算机程序包括计算机程序代码，计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。计算机可读介质可以包括：能够携带计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括是电载波信号和电信信号。

计算机可读存储介质可以是前述任一实施例的终端的内部存储单元，例如终端的硬盘或内存。计算机可读存储介质也可以是终端的外部存储设备，例如终端上配备的插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(Flash Card)等。进一步地，计算机可读存储介质还可以既包括终端的内部存储单元也包括外部存储设备。计算机可读存储介质用于存储计算机程序及终端所需的其他程序和数据。计算机可读存储介质还可以用于暂时地存储已经输出或者将要输出的数据。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，上述描述的终端和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

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

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

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

以上，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (10)

1.一种机器人轨迹跟踪方法，其特征在于，包括：

基于快速幂次趋近律确定机器人的横轴位置跟踪控制律和纵轴位置跟踪控制律；

根据所述横轴位置跟踪控制律和所述纵轴位置跟踪控制律确定机器人的线速度控制律；

基于饱和函数指数趋近律确定机器人的姿态控制律；

根据所述线速度控制律和所述姿态控制律对机器人轨迹进行跟踪。

2.如权利要求1所述的机器人轨迹跟踪方法，其特征在于，所述横轴位置跟踪控制律为：

其中，u₁为横轴位置跟踪控制律，为机器人横轴当前目标位置，k_x为横轴积分时间，x_e为横轴位置跟踪误差，ε_x为横轴第一控制律系数，η_x为横轴第二控制律系数；

其中，s_x为横轴积分滑模面：

3.如权利要求1所述的机器人轨迹跟踪方法，其特征在于，所述纵轴位置跟踪控制律为：

其中，u₂为纵轴位置跟踪控制律，为机器人纵轴当前目标位置，k_y为纵轴积分时间，y_e为纵轴位置跟踪误差，ε_y为纵轴第一控制律系数，η_y为纵轴第二控制律系数；

其中，s_y为横轴积分滑模面：

4.如权利要求1所述的机器人轨迹跟踪方法，其特征在于，所述线速度控制律的确定方法包括：

其中，θ_d为：

其中，u₁为横轴位置跟踪控制律，u₂为纵轴位置跟踪控制律。

5.如权利要求1所述的机器人轨迹跟踪方法，其特征在于，所述姿态控制律的确定方法包括：

其中，w为姿态控制律，为当前目标方向角度，k_θ为方向积分时间，s_θ为方向积分滑模面，η_θ为方向第一控制系数；

其中，sat(s_θ)为：

其中，φ(s_θ)是为了引进边界层而给出的边界。

6.一种机器人轨迹跟踪装置，其特征在于，包括：

横纵轴控制律确定模块，用于基于快速幂次趋近律确定机器人的横轴位置跟踪控制律和纵轴位置跟踪控制律；

线速度控制律确定模块，用于根据所述横轴位置跟踪控制律和所述纵轴位置跟踪控制律确定机器人的线速度控制律；

姿态控制律确定模块，用于基于饱和函数指数趋近律确定机器人的姿态控制律；

轨迹跟踪模块，用于根据所述线速度控制律和所述姿态控制律对机器人轨迹进行跟踪。

7.如权利要求6所述的机器人轨迹跟踪装置，其特征在于，所述横纵轴控制律确定模块包括横轴控制律确定单元，用于确定横轴位置跟踪控制律，所述横轴位置跟踪控制律为：

其中，u₁为横轴位置跟踪控制律，为机器人横轴当前目标位置，k_x为横轴积分时间，x_e为横轴位置跟踪误差，ε_x为横轴第一控制律系数，η_x为横轴第二控制律系数；

其中，s_x为横轴积分滑模面：

8.如权利要求6所述的机器人轨迹跟踪装置，其特征在于，所述横纵轴控制律确定模块包括纵轴控制律确定单元，所述纵轴控制律确定单元用于确定纵轴位置跟踪控制律，所述纵轴位置跟踪控制律为：

其中，u₂为纵轴位置跟踪控制律，为机器人纵轴当前目标位置，k_y为纵轴积分时间，y_e为纵轴位置跟踪误差，ε_y为纵轴第一控制律系数，η_y为纵轴第二控制律系数；

其中，s_y为横轴积分滑模面：

9.一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至5任一项所述方法的步骤。

10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至5任一项所述方法的步骤。