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

权利要求书

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、δ_θ都是正参数，即可使编队跟踪误差渐近收敛。