[发明专利]一种轮式机器人编队跟踪与避障控制方法

权利要求书

1.一种轮式机器人编队跟踪与避障控制方法，其特征在于：其包括如下步骤：

S1、建立实际轮式机器人和虚拟轮式机器人的运动学模型，采用领导-跟随法得出实际轮式机器人和虚拟轮式机器人的编队跟踪误差模型；

S2、根据人工势场法，设计障碍物的斥力势场函数，求出障碍物对实际轮式机器人的斥力大小；

S3、利用障碍物对实际轮式机器人的合斥力大小对轮式机器人编队跟踪误差大小进行修正，得到一条无碰撞路径；

S4、对编队跟踪任务和编队避障任务设置优先级排序，同时根据输入输出反馈线性化方法设计分布式编队控制器，使轮式机器人编队不仅能够按照期望队形跟踪参考轨迹运动，还具备实时自主避障的能力。

2.根据权利要求1所述的一种轮式机器人编队跟踪与避障控制方法，其特征在于：所述步骤S1建立实际轮式机器人和虚拟轮式机器人的运动学模型，采用领导-跟随法得出实际轮式机器人和虚拟轮式机器人的编队跟踪误差模型，具体步骤如下：

S1.1、建立实际轮式机器人和虚拟轮式机器人的运动学模型；

其中，x_i、y_i、θ_i分别为实际轮式机器人i(i＝1,…,n)在全局坐标系下x坐标、y坐标和方向角的位姿信息；v_i、ω_i分别表示实际轮式机器人i的线速度和角速度；

其中，分别为虚拟轮式机器人i^r(i＝1,…,n)在全局坐标系下x坐标、y坐标和方向角的位姿信息；x₀、y₀、θ₀分别为参考轨迹在全局坐标系下x坐标、y坐标和方向角的位姿信息；d表示轮式机器人的质心与其后轮连轴的距离；l_i、分别为预先设计的虚拟机器人相对于参考轨迹的质心距离参数和方位角参数；

S1.2、通过坐标转换，得出实际轮式机器人和虚拟轮式机器人的编队跟踪误差模型；

其中，表示虚拟轮式机器人位姿与实际轮式机器人位姿之间的偏差；

将上述编队跟踪误差模型求导得：

其中，v₀、ω₀分别为参考轨迹的线速度、角速度。

3.根据权利要求1所述的一种轮式机器人编队跟踪与避障控制方法，其特征在于：所述步骤S2中根据人工势场法设计障碍物的斥力势场函数，求出障碍物对实际轮式机器人的斥力函数，具体方法如下：

S2.1、根据人工势场法设计障碍物j(j＝1,…,m)的斥力势场函数；

其中，U_i,j表示障碍物j作用于实际轮式机器人i的斥力势场函数；α_j为大于0的可调斥力势场强度因子；R_j表示障碍物j的斥力势场区域半径；d_i,j为实际轮式机器人i与障碍物j的直线距离,即：这里x_j、y_j分别表示障碍物j在全局坐标系下的x轴、y轴的坐标信息；

S2.2、根据障碍物j的斥力势场函数，计算出障碍物j对实际轮式机器人i的斥力大小；

这里，

其中，F_i,j为障碍物j对轮式机器人i的斥力大小；分别为斥力势场函数U_i,j对变量x、y的偏导数；分别表示在全局坐标系下x轴、y轴的方向向量。

4.根据权利要求1所述的一种轮式机器人编队跟踪与避障控制方法，其特征在于：所述步骤S3利用障碍物对实际轮式机器人的合斥力大小对编队跟踪误差大小进行修正的具体步骤如下：

S3.1、根据力的叠加原理，计算出m个障碍物对实际轮式机器人i的合斥力大小如下：

其中，分别表示合斥力在全局坐标系下x轴、y轴的分量大小；

S3.2、通过坐标变换，利用合斥力大小对轮式机器人编队跟踪误差大小修正为如下形式：

其中，分别为修正后的编队x轴、y轴的跟踪误差大小。

5.根据权利要求1所述的一种轮式机器人编队跟踪与避障控制方法，其特征在于：所述步骤S4对编队跟踪任务和编队避障任务设置优先级排序，同时根据输入输出反馈线性化方法设计分布式编队控制器，使轮式机器人编队不仅能够按照期望队形跟踪参考轨迹运动，还具备自主避障的能力，具体步骤如下：

S4.1、对编队跟踪任务和编队避障任务设置优先级排序；

若实际轮式机器人未进入障碍物的斥力势场区域，即：当d_i,j≥R_j时，实际轮式机器人的任务优先级为：编队跟踪任务编队避障任务；此时，障碍物对实际轮式机器人的斥力大小为无需对编队跟踪误差进行修正；

若实际轮式机器人进入障碍物的斥力势场区域，即：当0d_i,jR_j时，实际轮式机器人的任务优先级为：编队避障任务编队跟踪任务；此时，障碍物对实际轮式机器人的斥力大小为需要对编队跟踪误差进行修正；

S4.2、根据输入输出反馈线性化方法设计分布式编队控制器；

其中，分别为实际轮式机器人i期望的线速度和角速度；k_x、k_y、k_θ分别为大于0的可调编队控制器参数。