CN109828580B - 一种基于分离式超声波的移动机器人编队跟踪控制方法 - Google Patents

Abstract

本发明公开了一种基于分离式超声波的移动机器人编队跟踪控制方法，包含步骤：步骤(1)：建立单个移动机器人的运动学模型；步骤(2)：建立n个移动机器人编队动态数学模型；步骤(3)：建立分离式超声波获取相对位姿的模型并推导出相对位姿的计算公式；步骤(4)：从编队跟踪控制的实际问题出发，结合所述相对位姿的计算公式设计出使编队跟踪误差渐近收敛的控制规律；步骤(5)：选取移动机器人参考路径，设置移动机器人参数与控制器参数，根据所述控制规律实现移动机器人的编队跟踪控制。本发明从实际应用角度出发，给出了一种分离式超声波获取位姿信息的方法，并设置实际可行的控制器，达到移动机器人编队跟踪控制误差渐近收敛的目标。

Description

一种基于分离式超声波的移动机器人编队跟踪控制方法

技术领域

本发明涉及移动机器人的编队控制领域，具体针对领导者跟随者编队方式中需要保持机器人之间运行的相对位姿，提出了一种基于分离式超声波的移动机器人编队跟踪控制方法。

背景技术

近几十年，随着科技的不断进步，生产生活中自动化智能化的程度不断提升，因此也激起了科研爱好者在这方面的不断探索，其中移动机器人就是这个领域中的一颗璀璨的星。从20世纪60年代斯坦福大学研究所的SHAKEY，70年代末期美国斯坦福大学和MIT合作的ALV研究，90年代后期CMU的NAVLAB移动机器人系统，移动机器人技术已经有了很大进步。现在，用于家庭清洁扫地机器人，服务于工厂中的AGV牵引车，用于太空探索的月球车等一系列移动机器人技术的产物逐渐出现在人们的视野。

移动机器人由于其本身能独立完成某项任务的能里，今后必然会在生产生活中发挥巨大的作用。例如太空探测，海洋探测，工业矿井探测这些由人类去完成危险程度很大甚至无法完成的任务，有了机器人的参与都将更好更快更安全地去完成。在有些大型的任务中，由于任务本身的复杂性，单个机器人由于自身能力有限，单独完成任务往往需要更多的时间和成本，而多个移动机器人互相合作，更高效，同时也具备更大的容错率。

控制移动机器人之间的良好协作关系，保证其在工作工程中稳定高效地完成每个任务，首要前提就是设计一种编队控制策略。编队控制的目标是设计控制器使得多个机器人能保持一个既定的编队几何形状，并能按预期完成任务。典型的编队控制方法有：基于行为控制、虚拟构造控制和领导者跟随者控制。每种方法都有其优缺点，而由于领导者跟随者编队控制方法的简单和易扩展性，本文使用该编队方法。在这种方法中，要求跟随者始终和领导者保持一定的位姿关系，因此相对位姿状态的获取和相对位姿状态的保持控制成为这个控制过程中关键环节。

移动机器人相对位姿信息的获取常见的有如下几种方式：(1)使用GPS获取全球位置坐标，并通过通信传递双方的位置信息，进而计算出相对位姿信息；(2)使用双目视觉获取周围环境图像信息，通过摄像机标定技术、图像匹配技术使用三角原理计算出相对位姿信息；(3)使用雷达循环扫描周围环境，通过计算发送接收时差获取相对位姿信息；(4)使用超声波模组发送超声波，超声波接收部分接收超声波，通过计算时差进而计算出相对位姿信息。在这些技术中，各有各的优缺点，其中GPS只能在室外使用，而且精度较低；图像虽然能获取丰富的信息，但是算法实现难度大，并且易受环境影响；而雷达的成本较高并且循环一次扫描的周期较长，实时性较差。而超声波具有成本较低，环境适应能力较强(超声波不受光线以及室内外等因素影响)，能够弥补GPS和图像获取相对位姿信息中存在的不足问题，但仍然存在精度不够、超声波模块需求较多等问题。

发明内容

本发明的目的在于解决移动机器人编队跟踪误差收敛问题，提出了一种基于分离式超声波的移动机器人编队跟踪控制方法，该方法从相对位姿的获取与相对位姿的保持上解决了移动机器人编队过程中领导者与跟随者避免碰撞与通信保持的问题，最终实现编队系统稳定运行的目标。

本发明目的通过以下技术方案实现:

一种基于分离式超声波的移动机器人编队跟踪控制方法，该方法包含步骤：

步骤(1)：建立单个移动机器人的运动学模型；

步骤(2)：建立n个移动机器人编队动态数学模型，并考虑实际系统中存在的相对位姿获取和相对位姿保持的问题；

步骤(3)：建立分离式超声波获取相对位姿的模型，并推导出相对位姿的计算公式；

步骤(4)：从编队跟踪控制的实际问题出发，结合推导出的所述相对位姿的计算公式设计出使编队跟踪误差渐近收敛的控制规律；

步骤(5)：选取移动机器人参考路径，设置移动机器人参数与控制器参数，根据所述控制规律实现移动机器人的编队跟踪控制。

进一步地，步骤(1)中，所述的建立单个机器人平面坐标的运动学模型具体包括：

假设移动机器人在运动过程中不发生侧滑，故满足如下条件：

得出移动机器人的运动学模型为：

其中，

来描述移动机器人R_i当前所处的位姿状态，x_i与y_i是二维坐标系统横轴纵轴的坐标位置，

是机器人R_i运动方向与横轴的夹角；(v_i,w_i)来描述移动机器人R_i当前所处运动状态，其中v_i是移动机器人R_i相对于全局坐标系的线速度，w_i是移动机器人R_i相对于全局坐标系的角速度。

进一步地，步骤(2)所述建立n个移动机器人编队动态数学模型具体包括：

定义移动机器人跟随者与领导者的跟随位姿偏差：

其中，d_i是跟随者机器人R_i与领导者机器人R_i-1间的距离，θ_i是跟随者机器人R_i与领导者机器人R_i-1的运动方向角度的偏差。x_i-1，y_i-1是领导者机器人R_i-1当前所处的全局坐标值，x_i，y_i是跟随者机器人R_i当前所处的全局坐标值，

定义如下公式所示：

然后求取偏差的动态方程：

其中v_i,w_i是跟随机器人R_i当前的线速度与角速度，v_i-1,w_i-1是领导者机器人R_i-1当前的线速度与角速度。

进一步地，步骤(3)中，所述分离式超声波的模组包括发射模块与接收模块，所述发射模块中同时设置有红外发送器和超声波发射器；所述接收模块中同时设置有红外接受器和超声波接收器，所述红外发送、接收器是用于实现时钟同步，所述发射模块同时控制红外发送器发射红外线以及超声波发射器发射超声波。

进一步地，步骤(3)中，每个机器人都安装有一个发射模块并放置在机器人中心(实际使用过程中一般将多个超声波发射头组装到一起形成一个超声波发射环，保证在R_i-1的周围R_i都能接收到超声波发射信号)；每个机器人都会在中心的两端安装两个所述接收模块，所述接收模块之间的距离为2_i，接收模块的连线方向与车前进的方向垂直。

进一步地，所述步骤(3)中建立分离式超声波获取相对位姿的模型、并推导出相对位姿的计算公式具体包括：

首先通过与常见的集成式超声波的测距对比，可得知分离式超声波的优点，通过数学建模后，故可以确定分离式超声波测距的计算公式为：

d_measure＝v_sound*t₃≈v_sound*(t₃-t₄)

其中d_measure是测量距离，v_sound是声音在空气中的传播速度，为一个常数，t₃是整个过程中声音传播的时间，t₄是整个过程中辅助的红外光传播的时间，t₃与t₄的差值可由超声波模组中的CPU计算获得；根据领导者跟随者超声波位置排布进行建模，基于过程中的图形关系，由余弦定理可以得到如下关系：

结合上述关于d_i与θ_i的定义，可以计算出d_i与θ_i为：

其中，2_i为跟随者机器人R_i左右超声波接收模块的距离，d_1i和d_2i分别为跟随者机器人R_i左右超声波接收模块测量获得的与领导机器人R_i-1中心的距离，β₁，β₂为推导过程中的中间变量。

进一步地，如步骤(4)具体包括：

设置移动机器人编队过程中距离约束与角度约束：

其中，d_max为最大约束距离，d_min为最小距离约束，d_max、d_min都是常数，由实际的传感器参数以及机器人的机械尺寸决定；θ_max为最大角度误差约束，θ_min为最小角度误差约束，其中θ_max、θ_min都是常数且θ_max＝-θ_min；

根据上述偏差定义，推导出动态误差方程为：

其中，

和

分别是机器人R_i与R_i-1运行方向与x轴的夹角；

采用障碍(barrier)李雅普诺函数的方法进行约束，从稳定性理论出发，设计编队跟踪控制误差渐近收敛的控制规律，用于保证跟踪误差全局渐近收敛，保证d_i收敛在d_min～d_max范围内，以及θ_i收敛在θ_min～θ_max范围内，故设定如下所示的李雅普诺夫函数为：

其中，β_d与β_θ是一个设置函数性能的参数，d_d与θ_d是d_i与θ_i的期望值，设置距离偏差与角度偏差的李雅普诺夫函数分别为V_d,V_θ，通过求解V_d，V_θ对d_i，θ_i的导数

与

分别为：

通过估计速度上界的方法，设置如下所示的控制规律：

其中，k_d，k_θ，δ_d，δ_θ都是正参数，

是对于领导者机器人速度上界的估计，tanh是双曲正切函数，结合控制规律以及上述

方程，可得：

结合上述控制规律与

动态方程，即可得：

通过设计k_d、k_θ、δ_d、δ_θ都是正参数，即可使

编队跟踪误差渐近收敛。

本发明与现有技术相比，有如下优点：

1、改进超声波获取距离的方法，给出了实际可行的计算策略。

2、使用障碍(barrier)李雅普诺夫函数对误差进行约束，从而保证提出的控制算法是能使编队跟踪误差渐近稳定且满足避免碰撞与保持通信前提的。

3、算法上对于机器人与x轴间的角度差进行放缩估计，对于领导机器人速度进行上界估计，减少对于其他传感器的依赖，更加具有实用价值。

附图说明

图1为集成式超声波模型原理图。

图2为分离式超声波模型原理图。

图3为分离式超声波获取相对位姿计算原理图。

图4为单机器人运动学模型图。

图5为n个机器人领导者跟随者编队跟踪模型图。

图6为编队跟踪误差偏差计算示意图。

图7至图8分别为虚拟机器人轨迹为圆时的编队跟踪示意图、位姿跟踪误差收敛图。

图9至图10分别为虚拟机器人轨迹为直线时的编队跟踪示意图、位姿跟踪误差收敛图。

图11为基于本发明的移动机器人编队跟踪控制方法的过程示意图。

具体实施方式

为了进一步阐述本发明实施的有效性，结合下面的实施例及附图，详细的说明具有预设性能和连接保持的水下机器人编队控制方法。

实施例

一种基于分离式超声波的移动机器人编队跟踪控制方法，该方法包含如下步骤：

步骤(1)：建立单个移动机器人的运动学模型；

步骤(2)：建立n个移动机器人编队动态数学模型，并考虑实际系统中存在的相对位姿获取和相对位姿保持的问题；

步骤(3)：建立分离式超声波获取相对位姿的模型，并推导出相对位姿的计算公式；

步骤(4)：从编队跟踪控制的实际问题出发，结合推导出的所述相对位姿的计算公式设计出使编队跟踪误差渐近收敛的控制规律；

步骤(5)：选取移动机器人参考路径，设置移动机器人参数与控制器参数，根据所述控制规律实现移动机器人的编队跟踪控制。

具体而言，在一个可行的实施例中，考虑如图4所示的三轮移动机器人模型，其中后轮是两个独立控制驱动轮，前轮是一个无驱动的万向轮。考虑全局坐标系中的移动机器人，通过

来描述机器人R_i当前所处的状态，其中x_i与y_i是二维坐标系统横轴纵轴的坐标大小，

是机器人运行方向与横轴的夹角，因此，步骤(1)中，所述的建立单个机器人平面坐标的运动学模型具体包括：

假设移动机器人在运动过程中不发生侧滑，故满足如下条件：

实际运动过程中移动机器人的运动状态可用(v_i,w_i)描述，其中v_i是移动机器人R_i相对于全局坐标系的线速度，w_i是移动机器人R_i相对于全局坐标系的角速度，故得出移动机器人的运动学模型为：

其中，

来描述移动机器人R_i当前所处的位姿状态，x_i与y_i是二维坐标系统横轴纵轴的坐标位置，

是机器人R_i运动方向与横轴的夹角；(v_i,w_i)来描述移动机器人R_i当前所处运动状态，其中v_i是移动机器人R_i相对于全局坐标系的线速度，w_i是移动机器人R_i相对于全局坐标系的角速度。

实际的控制器中的控制量为左右驱动轮的速度(v_li,v_ri)，其中v_li为左驱动轮的转速，v_ri为右驱动轮的转速，与(v_i,w_i)的转换关系如下：

其中a_i为左右轮间距离的一半，r_i为驱动轮的半径。

具体而言，在本发明的可行的实施例中，移动机器人编队跟踪过程中考虑，考虑一个领导者和一群跟随者形成的一组(共n个)移动机器人群体R₁～R_n，并形成如图2所示的配置，编队算法中考虑的领导者跟随者队形配置的特征可以概括如下：

有一个虚拟参考机器人标记R₀，提供领航参考轨迹，实际的轨迹情况应该由领导机器人的SLAM算法提供，涉及到障碍物识别，最优路径规划等，这里我们暂时不做讨论。

在编队控制中，标记为R_i的机器人(其中i∈n)始终跟随其唯一的固定的领导机器人R_i-1,在运行过程中跟随机器人R_i通过分离式超声波获取与领导机器人R_i-1的相对距离，并实时计算距离d_i与角度偏差θ_i。而这些传感器的检测距离是有限，因此我们需要对机器人之间的最大距离进行约束，此外还应保证移动机器人在运行过程中避免发生碰撞而损坏设备，因此同时需要对机器人间的最小距离进行约束。考虑如图5所示的领导机器人与跟随机器人的配置关系，因此，步骤(2)所述建立n个移动机器人编队动态数学模型具体包括：

定义移动机器人跟随者与领导者的跟随位姿偏差：

其中，d_i是跟随者机器人R_i与领导者机器人R_i-1间的距离，θ_i是跟随者机器人R_i与领导者机器人R_i-1的运动方向角度的偏差。x_i-1，y_i-1是领导者机器人R_i-1当前所处的全局坐标值，x_i，y_i是跟随者机器人R_i当前所处的全局坐标值，

定义如下公式所示：

然后求取偏差的动态方程：

其中v_i,w_i是跟随机器人R_i当前的线速度与角速度，v_i-1,w_i-1是领导者机器人R_i-1当前的线速度与角速度。

具体而言，步骤(3)中，所述分离式超声波的模组包括发射模块与接收模块，所述发射模块中同时设置有红外发送器和超声波发射器；所述接收模块中同时设置有红外接受器和超声波接收器，所述红外发送、接收器是用于实现时钟同步，所述发射模块同时控制红外发送器发射红外线以及超声波发射器发射超声波。如图3所示，每个机器人都安装有一个发射模块并放置在机器人中心(实际使用过程中一般将多个超声波发射头组装到一起形成一个超声波发射环，保证在R_i-1的周围R_i都能接收到超声波发射信号)；每个机器人都会在中心的两端安装两个所述接收模块，所述接收模块之间的距离为2_i，接收模块的连线方向与车前进的方向垂直。

进一步地，所述步骤(3)中建立分离式超声波获取相对位姿的模型、并推导出相对位姿的计算公式具体包括：

在许多工程应用中我们较为常见的超声波模块测量与目标物体的距离，将其建模为如图1所示，假设超声波发射模块到发送超声波到超声波接触目标物的时间为t₁，超声波从目标物反射到超声波接收模块的时间为t₂，可知t₁＝t₂。超声波模块内的CPU能够计算超声波发射到超声波接收之间的时间差t，可知：

t＝t₁+t₂

并可以计算出超声波模块到目标物的距离d_measure为：

d_measure＝v_sound*t/2

其中v_sound为声音在空气中的传播速度，一般取v_sound＝340m/s。

但是使用这种方法用于测量跟随机器人R_i与领导机器人R_i-1之间的相对位姿存在一定的局限性，因为在移动机器人运行过程中跟随机器人R_i很难接收到领导机器人R_i-1反射回的超声波，并且对于相对角度的获取也是不理想的，因此提出使用分离式超声波模块，并将其建模成如图2所示，可知，光速v_light＝300000km/s,而声音的传播速度v_sound＝340m/s。故可知：

v_light＞＞v_sound,t₃＞＞t₄

因此声音传播的时间t₃远大于光的传播时间t₄，即光传播时间可以忽略,故通过数学建模后，确定分离式超声波测距的计算公式为：

d_measyre＝v_sound*t₃≈v_sound*(t₃-t₄)

其中d_measure是测量距离，v_sound是声音在空气中的传播速度，为一个常数，t₃是整个过程中声音传播的时间，t₄是整个过程中辅助的红外光传播的时间，t₃与t₄的差值被接收模块测量并计算出来；

根据领导者跟随者超声波位置排布进行建模，基于过程中的图形关系，由余弦定理可以计算出R_i的左右超声波接收装置距离R_i-1中心的距离为d_1i和d_2i，得到如下关系：

结合上述关于d_i与θ_i的定义，可以计算出d_i与θ_i为：

其中，2b_i为跟随者机器人R_i左右超声波接收模块的距离，d_1i和d_2i分别为跟随者机器人R_i左右超声波接收模块测量获得的与领导机器人R_i-1中心的距离，β₁，β₂为推导过程中的中间变量。

具体而言，在可行的实施例中，所述步骤(4)具体包括：

设置移动机器人编队过程中距离约束与角度约束，实际距离d_i，实际角度差θ_i应该满足如下关系为：

其中，d_max为最大约束距离，d_min为最小距离约束，d_max、d_min都是常数，由实际的传感器参数以及机器人的机械尺寸决定；此外由于传感器的实际观测角度限制，因此角度误差也存约束；即，θ_max为最大角度误差约束，θ_min为最小角度误差约束，其中θ_max、θ_min都是常数且θ_max＝-θ_min；

考虑如图6所示的领导机器者跟随者配置，本文将从运动学上设计误差收敛的控制器，因此，根据上述偏差定义，推导出动态误差方程为：

其中，

和

分别是机器人R_i与R_i-1运行方向与x轴的夹角，由于在分离式超声波获取相对位姿信息时，这两者的偏差无法获取，因此在控制器设计时应试图放缩掉这部分。其次在控制器设计过程中尽量减低系统对于其他观测量的要求，因此对于领导者的速度尽量采用估计的方法，由于要保证d_i收敛在d_min～d_max范围内以及θ_i能够收敛在θ_min～θ_max范围内，因此采用障碍(barrier)李雅普诺函数的方法进行约束，从稳定性理论出发，设计编队跟踪控制误差渐近收敛的控制规律，用于保证跟踪误差全局渐近收敛，保证d_i收敛在d_min～d_max范围内，以及θ_i收敛在θ_min～θ_max范围内，故设定如下所示的李雅普诺夫函数为：

其中，β_d与β_θ是一个设置函数性能的参数，d_d与θ_d是d_i与θ_i的期望值，由障碍李雅普诺夫函数的性质可以看出，只要设计出全局收敛的控制器，并且满足初始条件d_min≤d_i(0)≤d_max，θ_min≤θ_i(0)≤θ_max，那么便能保证距离约束与角度约束的条件：d_min≤d_i≤d_max，θ_min≤θ_i≤θ_max，其中β_d与β_θ是一个设置函数性能的参数，d_d与θ_d是d_i与θ_i的期望值，在参数设计合理时d_i与θ_i会收敛到这两个值。

设置距离偏差与角度偏差的李雅普诺夫函数分别为V_d,V_θ，通过求解V_d，V_θ对d_i，θ_i的导数

与

分别为：

通过估计速度上界的方法，设置如下所示的控制规律：

其中，k_d，k_θ，δ_d，δ_θ都是正参数，

是对于领导者机器人速度上界的估计，tanh是双曲正切函数，结合控制规律以及上述

方程，可得：

结合上述控制规律与

动态方程，即可得：

显然，通过设计k_d、k_θ、δ_d、δ_θ都是正参数，即可使

由李雅普诺夫稳定性理论可知，编队跟踪误差渐近收敛。

具体而言，如图11所示，步骤(5)中，根据所述控制规律实现移动机器人的编队跟踪控制时，实际机器人系统都为离散控制系统，故本实施例中，对于机器人R_i，最终通过求解运动学模型函数的微分方程更新位姿。整个机器人编队控制系统中的收敛轨迹实际上由虚拟机器人R₀的全局算法来决定，因此只要能保证跟踪算法的全局稳定，那么R_i的轨迹最终将收敛于R_i-1的运动轨迹，并满足距离约束与角度约束条件。本实施例中分别对R₀轨迹为直线，圆两种情况进行仿真实验，本实施例中共采用三个移动机器人的编队跟踪控制仿真，其中：

(1)其中虚拟机器人R₀为直线轨迹的情况时：

机器人的R₁～R₃起始点的位姿信息

分别为：

(1.05,-0.15,π/2),(1.10,-0.30,π/2),(1.5,-0.45,π/2)

机器人的R₁～R₃起始点的速度信息(v,w)分别为：

(0.2,0),(0.2,0),(0.2,0)

虚拟机器人R₀轨迹方程为：

编队跟踪控制规律中控制器参数为：

d_max＝3,d_min＝0.02,d_d＝0.1,,θ_max＝1,θ_min＝-1,θ_d＝0

β_d＝14,β_θ＝1,δ_d＝0.35,δ_θ＝0.39,

k_d＝0.042,k_θ＝0.49

(2)其中虚拟机器人R₀为圆轨迹的情况时：

机器人的R₁～R₃起始点的位姿信息

分别为：

(1.05,-0.15,pi/2),(1.10,-0.30,pi/2),(1.5,-0.45,pi/2)

机器人的R₁～R₃起始点的速度信息(v,w)分别为：

(0.2,0.2),(0.2,0.2),(0.2,0.2)

虚拟机器人R₀轨迹方程为：

x(t)＝cos(0.1t)；y(t)＝sin(0.1t)；T＝0.1s

编队跟踪控制规律中控制器参数为：

d_max＝3,d_min＝0.02,d_d＝0.1,,θ_max＝1,θ_min＝-1,θ_d＝0

β_d＝15,β_θ＝1,δ_d＝0.35,δ_θ＝0.39,

k_d＝0.042,k_θ＝0.49

图7至图10分别刻画了在不同的机器人轨迹下多机器人轨迹跟踪以及误差收敛的结果。能看出跟踪误差能较快地收敛到零。所以在本发明方法下设计的控制方法具有很好的稳定性。

本发明提出了的基于分离式超声波的移动机器人编队跟踪控制方法，与常见的集成式超声波模块相比具有计算更精确角度信息的特点，而且利用在移动机器人编队跟踪上能使用更少的超声波模块，因此这是一种非常具有实用价值意义的方法。

移动机器人相对位姿信息的保持主要是相对距离与相对角度的控制，因此主要就是通过设置一个控制器实现相对位姿的误差收敛。移动机器人系统是一个欠驱动系统，本发明通过使用李雅普诺夫稳定性理论设计出一个保证跟踪误差渐进收敛的控制器。

本发明通过设计一种分离式超声波获取相对位姿并用于编队控制跟踪的控制方法，从相对位姿的信息获取与相对位姿状态的保持解决移动机器人编队控制跟踪过程中的误差收敛问题，最终达到移动机器人编队系统稳定运行的目标。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (2)

1.一种基于分离式超声波的移动机器人编队跟踪控制方法，其特征在于，包含步骤：

步骤(1)：建立单个移动机器人的运动学模型；

步骤(2)：建立n个移动机器人编队动态数学模型，并考虑实际系统中存在的相对位姿获取和相对位姿保持的问题；

步骤(3)：建立分离式超声波获取相对位姿的模型，并推导出相对位姿的计算公式；

步骤(4)：从编队跟踪控制的实际问题出发，结合推导出的所述相对位姿的计算公式设计出使编队跟踪误差渐近收敛的控制规律；

步骤(5)：选取移动机器人参考路径，设置移动机器人参数与控制器参数，根据所述控制规律实现移动机器人的编队跟踪控制；

步骤(1)中，所述的建立单个机器人平面坐标的运动学模型具体包括：

假设移动机器人在运动过程中不发生侧滑，故满足如下条件：

得出移动机器人的运动学模型为：

其中，

来描述移动机器人R_i当前所处的位姿状态，x_i与y_i是二维坐标系统横轴纵轴的坐标位置，

是机器人R_i运动方向与横轴的夹角；(v_i,w_i)来描述移动机器人R_i当前所处运动状态，其中v_i是移动机器人R_i相对于全局坐标系的线速度，w_i是移动机器人R_i相对于全局坐标系的角速度；

步骤(2)所述建立n个移动机器人编队动态数学模型具体包括：

定义移动机器人跟随者与领导者的跟随位姿偏差：

其中，d_i是跟随者机器人R_i与领导者机器人R_i-1间的距离，θ_i是跟随者机器人R_i与领导者机器人R_i-1的运动方向角度的偏差；x_i-1，y_i-1是领导者机器人R_i-1当前所处的全局坐标值，x_i，y_i是跟随者机器人R_i当前所处的全局坐标值，

为两中间变量定义如下公式所示：

然后求取偏差的动态方程：

其中v_i,w_i是跟随机器人R_i当前的线速度与角速度，v_i-1,w_i-1是领导者机器人R_i-1当前的线速度与角速度；

步骤(3)中，每个机器人都安装有一个发射模块并放置在机器人中心；每个机器人都会在中心的两端安装两个接收模块，所述接收模块之间的距离为2b_i，接收模块的连线方向与车前进的方向垂直；

所述发射模块中同时设置有红外发送器和超声波发射器；所述接收模块中同时设置有红外接受器和超声波接收器，所述红外发送、接收器是用于实现时钟同步，所述发射模块同时控制红外发送器发射红外线以及超声波发射器发射超声波；

步骤(3)中建立分离式超声波获取相对位姿的模型、并推导出相对位姿的计算公式具体包括：

首先通过与常见的集成式超声波的测距对比，可得知分离式超声波的优点，通过数学建模后，故确定分离式超声波测距的计算公式为：

d_measure＝v_sound*t₃≈v_sound*(t₃-t₄)

其中d_measure是测量距离，v_sound是声音在空气中的传播速度，为一个常数，t₃是整个过程中声音传播的时间，t₄是整个过程中辅助的红外光传播的时间，t₃与t₄的差值可由超声波模组中的CPU计算获得；根据领导者跟随者超声波位置排布进行建模，基于过程中的图形关系，由余弦定理得到如下关系：

结合上述关于d_i与θ_i的定义，计算出d_i与θ_i为：

其中，2b_i为跟随者机器人R_i左右超声波接收模块的距离，d_1i和d_2i分别为跟随者机器人R_i左右超声波接收模块测量获得的与领导机器人R_i-1中心的距离，β₁，β₂为推导过程中的中间变量。

2.根据权利要求1所述的方法，其特征在于，步骤(4)具体包括：

设置移动机器人编队过程中距离约束与角度约束：

其中，d_max为最大约束距离，d_min为最小距离约束，d_max、d_min都是常数，由实际的传感器参数以及机器人的机械尺寸决定；θ_max为最大角度误差约束，θ_min为最小角度误差约束，其中θ_max、θ_min都是常数且θ_max＝-θ_min；

根据上述移动机器人跟随者与领导者的跟随位姿偏差，推导出动态误差方程为：

其中，

和

分别是机器人R_i与R_i-1运行方向与x轴的夹角；

采用障碍(barrier)李雅普诺函数的方法进行约束，从稳定性理论出发，设计编队跟踪控制误差渐近收敛的控制规律，用于保证跟踪误差全局渐近收敛，保证d_i收敛在d_min～d_max范围内，以及θ_i收敛在θ_min～θ_max范围内，故设定如下所示的李雅普诺夫函数V_d(d_i)与V_θ(θ_i)为：

其中，β_d与β_θ是一个设置函数性能的参数，d_d与θ_d是d_i与θ_i的期望值，设置距离偏差与角度偏差的李雅普诺夫函数分别为V_d,V_θ，通过求解V_d，V_θ对d_i，θ_i的导数

与

分别为：

通过估计速度上界的方法，设置如下所示的控制规律：

其中，k_d，k_θ，δ_d，δ_θ都是正参数，

是对于领导者机器人速度上界的估计，tanh是双曲正切函数，结合控制规律以及上述

方程，可得：

结合上述控制规律与

动态方程，即可得：

通过设计k_d、k_θ、δ_d、δ_θ都是正参数，即可使

编队跟踪误差渐近收敛。

Images