[发明专利]一种基于单应性的移动机器人编队跟随方法

权利要求书

1.一种基于单应性的移动机器人编队跟随方法，用于移动机器人在多机器人协作系统中的编队跟随控制，该方法采用领航-跟随模型，所述的领航机器人和跟随机器人均为轮式机器人，且跟随机器人配置单目相机可实时采集图像信息，其特征在于，包括如下步骤：

步骤1：在初始时刻，给定表示理想队形中跟随机器人位姿的期望图像和距离信息，利用跟随机器人上安装的单目相机获取当前图像，作为初始图像，根据期望图像与初始图像计算单应性矩阵并记录；

步骤2：在步骤1中得到初始、期望两幅图之间的单应性矩阵后，在初始图像属于领航机器人的且与相机光轴垂直的某一平面区域中选取若干特征点；

步骤3：给定信号让跟随机器人开始运行，运行过程中不断对这些特征点进行跟踪以此来计算当前图像与期望图像之间的单应性矩阵，进一步依据单应性矩阵元素构造系统误差；

所述的单应性矩阵的计算公式如下：

其中(x_e,z_e,θ_e)表示跟随机器人与其期望位置的相对位姿，d^*表示期望位置到所述的平面区域的距离即理想队形中跟随机器人与领航机器人的间距；

所述的系统误差由如下公式计算：

其中h₁₁,h₁₃,h₃₁,h₃₃为单应性矩阵中的元素；

步骤4：根据步骤3中计算得到的当前图像与期望图像的单应性矩阵计算并记录本次运行周期中跟随机器人的线速度和角速度，当运行过一个周期计算出下一个当前图像与期望图像之间单应性矩阵；结合上一周期和当前周期的单应性矩阵以及上一周期的跟随机器人的线速度和角速度，根据如下公式来估计运行过程中领航机器人的移动线速度和角速度：

其中(v_f,ω_f)为当前运行周期跟随机器人的线速度与角速度；

步骤5：将步骤3得到的实时系统误差和步骤4估计出的领航机器人的线速度和角速度共同作为控制器的输入信号，再由控制器输出信号驱动跟随机器人运动，运行过程中领航机器人始终自主运动；

其中，控制器输出用于驱动跟随机器人的速度信号由如下公式计算：

其中k₁,k₂是控制增益，根据实际实验取值；

步骤6：跟随机器人接收驱动信号进行移动，同时利用单目相机获取实时当前图像，重复进行步骤3、4、5，经过这些步骤后跟随机器人将逐渐与领航机器人形成期望图像所指定的理想队列。