一种非标准连接拖挂式移动机器人方向角控制方法及系统与流程

本公开涉及机器人方向角控制技术领域，特别涉及一种非标准连接拖挂式移动机器人方向角控制方法及系统。

背景技术：

本部分的陈述仅仅是提供了与本公开相关的背景技术，并不必然构成现有技术。

拖挂式移动机器人由位于最前端的牵引机器人(tractor)和由之拖动的多个被动的拖挂式机器人(trailer)组成，牵引机器人执行转向和驱动功能，而拖挂式机器人跟随牵引机器人运动。拖挂式移动机器人一方面具有移动机器人的运动能力，另一方面可以拓展移动机器人的其它功能。拖挂式移动机器人具有较高的应用价值。目前，拖挂式移动机器人的研究对象主要为室内服务机器人，如清洁机器人，救援机器人，在工厂仓库、机场用于运输货物、搬运行李、货物中转等任务的运输机器人。例如清洁机器人，可以在移动机器人后拖挂一个清扫和收集垃圾的卡车，从而降低成本，提高效率；另外，救援机器人可以通过拖挂增加其运输能力，同时拖挂式机器人节数也可以灵活改变，以便运输大量物体。

国际机器人技术联合会对服务机器人有一个初步的定义：服务机器人是一种半自主或全自主工作的机器人，它能完成有益于人类的服务工作，但不包括从事生产的设备。从广义上可以认为服务机器人是指除工业机器人之外的各种机器人，主要应用于服务业，如水下机器人、清洁机器人、救援机器人等。目前，服务机器人正在以飞快的速度发展壮大。近几年服务类机器人在国内市场呈现高速增长趋势。

针对标准连接的拖挂式移动机器人，根据链式幂零系统，利用正弦时变方法，可使系统从初始状态运动到任意所需的状态，有研究人员提出引入了平面度和线性输出的概念，以此来解决带有两节拖挂式机器人的移动机器人的停车问题；对于未转换的系统，有研究人员提出一种用于带有n节拖挂式机器人的机器人矢量场定向控制器，它可以引导机器人稳定地运动到给定点。带有拖挂式机器人的移动机器人的路径和轨迹跟踪问题也得到了很多专家的研究；有研究人员专为卡车和拖挂式机器人设计了混合控制器，它可以稳定的跟踪轨迹从而避免了jack-knife的影响；有研究人员利用lyapunov技术为单轴拖挂式机器人提出了一个渐近稳定的路径跟踪控制器；有研究人员提出了一种鲁棒自适应反馈线性化动态控制器来跟踪根据移动机器人运动学模型规划出的参考轨迹；针对标准连接的拖挂式移动机器人，有研究人员提出一种n节拖挂式移动机器人反向运动时的方向角控制方法。

本公开发明人发现，非标准连接的拖挂式移动机器人研究相对较少，从控制问题研究角度来看，由于拖挂式移动机器人系统的内在特性，如运动学模型高度非线性，非完整约束，结构奇异和反向运动时的不稳定性，带有拖挂式机器人的移动机器人的反向运动控制问题难度较大。

技术实现要素：

为了解决现有技术的不足，本公开提供了一种非标准连接拖挂式移动机器人方向角控制方法及系统，通过设定牵引机器人转向角控制律，控制牵引机器人按照设定转向角运动，即可拖动拖挂式机器人按照期望方向角进行平稳的反向运动。

为了实现上述目的，本公开采用如下技术方案：

本公开第一方面提供了一种非标准连接拖挂式移动机器人方向角控制方法。

一种非标准连接拖挂式移动机器人方向角控制方法，根据获取的牵引机器人和拖挂式机器人的参数，得到与拖挂式机器人和牵引机器人当前夹角以及期望方向角相关的牵引机器人转向角控制律，牵引机器人按照设定转向角运动，拖动拖挂式机器人按照期望方向角进行平稳的反向运动。

作为可能的一些实现方式，所述转向角控制律，具体为：

其中，γ1为牵引机器人和拖挂式机器人之间的夹角，γd为拖挂式机器人夹角与期望方向角之差，s1为定义的辅助变量，k1为控制系数，为牵引机器人的前轮转向角，l1牵引机器人的轴距，lt拖挂式机器人的连杆长度，l2拖挂式机器人的车身长度。

作为进一步的限定，建立拖挂式移动机器人的运动学模型，具体为：

其中，分别为牵引机器人的后轴中心的横坐标和纵坐标的导数，θ1为牵引机器人的运动方向与x方向的夹角，θ2为拖挂式机器人的运动方向与x方向的夹角，和分别为θ1和θ2的导数，v为牵引机器人后轮的线速度。

作为更进一步的限定，定义辅助变量，具体为：

s0＝sinγd

s1＝sinγd-sinγ1

进一步的，得到s0和s1的导数为：

作为更进一步的限定，所述k1，计算方式具体为：

作为更进一步的限定，和构成一个互联系统，具体为：

其中，

g2(s)＝0。

作为更进一步的限定，根据互联系统定理结论，当t→∞时，有s0→0和s1→0；

进一步的，当s0→0和s1→0时，有γd→0，γ1→0，即θ1→θd，θ2→θd，即通过控制牵引机器人的前轮转向角即可控制拖挂式移动机器人稳定的反向跟踪期望方向角θd。

本公开第二方面提供了一种非标准连接拖挂式移动机器人方向角控制系统。

一种非标准连接拖挂式移动机器人方向角控制系统，包括牵引机器人和拖挂式机器人，还包括：

数据采集模块，被配置为：实时采集牵引机器人和拖挂式机器人的参数；

控制律构建模块，被配置为：根据获取的牵引机器人和拖挂式机器人的参数，得到与拖挂式机器人和牵引机器人当前夹角以及期望方向角相关的牵引机器人转向角控制律；

运动控制模块，被配置为：牵引机器人按照设定转向角运动，拖动拖挂式机器人按照期望方向角进行平稳的反向运动；

其中，所述转向角控制律，具体为：

其中，γ1为牵引机器人和拖挂式机器人之间的夹角，γd为拖挂式机器人夹角与期望方向角之差，s1为定义的辅助变量，k1为控制系数，为牵引机器人的前轮转向角，l1牵引机器人的轴距，lt拖挂式机器人的连杆长度，l2拖挂式机器人的车身长度。

本公开第三方面提供了一种可读存储介质，其上存储有程序，该程序被处理器执行时实现如本公开第一方面所述的非标准连接拖挂式移动机器人方向角控制方法中的步骤。

本公开第四方面提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，所述处理器执行所述程序时实现如本公开第一方面所述的非标准连接拖挂式移动机器人方向角控制方法中的步骤。

与现有技术相比，本公开的有益效果是：

1、本公开所述的内容通过设定牵引机器人转向角控制律，控制牵引机器人按照设定转向角运动，即可拖动拖挂式机器人按照期望方向角进行平稳的反向运动，解决了非标准连接拖挂式移动机器人的方向跟踪控制问题，为非标准连接拖挂式移动机器人反向运动控制提供了理论指导，有益效果显著，适于应用推广。

2、本公开所述的转向角控制律完全依赖于牵引机器人和拖挂式机器人的运动参数和尺寸参数，不需要引入额外的参数，极大的提高了机器人方向控制的准确度。

附图说明

图1为本公开实施例1提供的非标准连接拖挂式移动机器人的结构示意图。

具体实施方式

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

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

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

实施例1：

本公开实施例1提供了一种非标准连接拖挂式移动机器人方向角控制方法，本实施例所述的拖挂式移动机器人由一牵引机器人和非标准连接的一节拖挂式机器人构成，如图1所示。

牵引机器人为前轮转向后轮驱动，设牵引机器人的前轮转向角为后轮的线速度为v。定义当时前轮转向左边，当时前轮转向右边；拖挂式机器人通过车轴与前面牵引机器人连接，设牵引机器人的轴距为l1，拖挂式机器人的连杆长度为lt，拖挂式机器人的车身长度为l2。pi(xi,yi)(i＝1,2)是牵引机器人和拖挂式机器人的后轴中心点坐标，用以描述牵引机器人和拖挂式机器人在笛卡尔坐标系中的位置。在本实施例中取l1＝1m，lt＝0.5m，l2＝2m。

本实施例具体通过以下步骤来实现：

(a)建立拖挂式移动机器人的运动学模型：

其中，x1、y1所组成的点(x1、y1)是牵引机器人的后轴中心的位置坐标；γ1是牵引机器人和拖挂式机器人之间的夹角，γ1＝θ1-θ2。

定义期望方向角为θd，且θd为常值，即且满足θ2(0)为拖挂式机器人方向角度初始值。在本实施例中拖挂式移动机器人的初值状态设置为：x1＝0,y1＝0,θ1(0)＝π,θ2(0)＝π。

(b)定义辅助变量

s0＝sinγd(2)

s1＝sinγd-sinγ1(3)

其中，γd＝θ2-θd。

(c)由系统(1)，推导出s0和s1的导数为：

设计控制律：

则有：

将(7)带入(5)中，s1的导数可以重写为：

(d)公式(4)和(8)可以构成一个形如公式(9)描述的互联系统。

其中，

g2(s)＝0。

对于公式(9)描述的互联系统，有如下定理：

如果系统(9)满足以下条件：

矩阵s为m矩阵：

其中αi和βi为正常数，φi(·)是正定且连续的函数，则互联系统(9)在系统的原点x＝0是渐近稳定的。

对于公式(4)和(8)所构成的互联系统，定义李雅普诺夫函数：和则函数f1(s0)和f2(s1)满足以下条件:

其中，φ1(s0)＝|s0|，φ2(s1)＝|s1|；且v1和v2满足；

其中，β1＝1，β2＝1。

对于函数g1(s)，有：

由于：

代入(13)式，得到：

即：

|g1(s)|≤γ11φ1(s0)+γ12φ2(s1)(15)

其中

对于函数g2(s)，由于g2(s)＝0，有：

|g2(s)|≤γ21φ1(s0)+γ22φ2(s1)(16)

其中γ21和γ22是任意非负常数。

(e)定义矩阵

其中,

可知s矩阵的顺序主子式

令则有：

选择c1<k1，则|s|>0，即s是一个m矩阵。

(f)根据互联系统的定理，两个互联子系统(4)和(8)满足条件c1～c5，根据互联系统定理结论，因此系统(4)和(8)在原点是渐近稳定的，即：当t→∞时，有s0→0和s1→0。

根据定义(2)和(3)，当s0→0和s1→0时，有γd→0，γ1→0，即θ1→θd，θ2→θd，即通过控制牵引机器人的前轮转向角即可控制拖挂式移动机器人稳定的反向跟踪期望方向角θd。

实施例2：

本公开实施例2提供了一种非标准连接拖挂式移动机器人方向角控制系统，包括牵引机器人和拖挂式机器人，还包括：

数据采集模块，被配置为：实时采集牵引机器人和拖挂式机器人的参数；

控制律构建模块，被配置为：根据获取的牵引机器人和拖挂式机器人的参数，得到与拖挂式机器人和牵引机器人当前夹角以及期望方向角相关的牵引机器人转向角控制律；

运动控制模块，被配置为：牵引机器人按照设定转向角运动，拖动拖挂式机器人按照期望方向角进行平稳的反向运动；

其中，所述转向角控制律，具体为：

其中，γ1为牵引机器人和拖挂式机器人之间的夹角，γd为拖挂式机器人夹角与期望方向角之差，s1为定义的辅助变量，k1为控制系数，为牵引机器人的前轮转向角，l1牵引机器人的轴距，lt拖挂式机器人的连杆长度，l2拖挂式机器人的车身长度。

实施例3：

本公开实施例3提供了一种可读存储介质，其上存储有程序，该程序被处理器执行时实现如本公开实施例1所述的非标准连接拖挂式移动机器人方向角控制方法中的步骤。

实施例4：

本公开实施例4提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，所述处理器执行所述程序时实现如本公开实施例1所述的非标准连接拖挂式移动机器人方向角控制方法中的步骤。

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