一种基于单应性的移动机器人编队跟随方法

摘要

本发明公开一种基于单应性的移动机器人编队跟随方法，该方法在给定理想队形间隔距离和理想期望图像的前提下，利用单应性矩阵构造可反映理想队形中跟随机器人实时位姿的虚拟机器人，将原先的编队问题转化为对虚拟机器人的轨迹跟踪问题。编队跟随过程中，领航机器人的速度采用估计的方式，利用单应性与速度之间的关系模型以及跟随机器人的实时速度能较为准确的估计领航速度，从而避免采用局部通信的方式，节省了编队实验成本。本方法简单可行，能满足移动机器人编队跟随的要求。

说明书

技术领域

本发明涉及一种轨迹跟踪方法，尤其涉及一种基于单应性的移动机器人编队跟随方法。

背景技术

移动机器人编队控制是一类典型的多机器人协作问题，在军事、生活、工业等方面具有广泛的应用前景，这类问题研究主要集中于调度、跟踪等智能交通领域，近几十年来一直是多机器人协作相关研究的热点和难点，该问题的研究对于多机器人需要共同完成作业时起到重要的作用。

编队跟随本质上属于轨迹跟踪问题，因此目前大多数编队控制的研究是基于轨迹跟踪研究的基础上进行的，传统的研究方法主要有：将原先非线性系统分解为多个低阶子系统，对子系统设计部分李雅普诺夫函数进而设计跟踪控制器，或者通过将系统转换为串联或链式系统形式，利用反步法设计轨迹跟踪控制器等。这些传统方式均是在机器人位姿信息已知的前提下进行的，实际场景中需要考虑机器人位姿信息的获取，通常采用激光、雷达等距离传感器或者建立局部通信网络等方式实现。

近些年基于单目相机的机器人视觉伺服控制快速发展，使得一些相关研究逐渐围绕单目视觉反馈来实现，常见的方法有：利用单目相机拍摄一系列周边图像，再辅以激光传感器获取的深度信息，构造出局部三维地图实时定位机器人位姿信息，进而完成编队任务，其成本较为高昂且控制器设计难度高；通过附加标记点来估计相关位姿，前提是需要事先标记且记录相关信息，提供较多先验知识；基于多视图几何的方法更为广泛，如极点和单应性分解，但是同样存在诸如极点奇异或分解不唯一等影响系统稳定性的问题。

发明内容

本发明针对现有技术的不足，提供一种基于单应性的移动机器人编队跟随方法，该方法的具体技术方案如下：

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

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

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

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

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

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

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

优选地，步骤3中单应性矩阵的计算公式如下：

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

优选地，步骤3中所构造的系统误差由如下公式计算：

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

优选地，步骤4中可根据如下公式来估计运行过程中领航机器人的移动线速度和角速度：

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

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

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

本发明的有益效果：本发明利用基于特征点跟踪的方式结合单应性传递特性来获取系统误差，提高了系统运行的实时性，利用单应性与速度之间的关系模型以及跟随机器人的实时速度较为准确地估计领航机器人速度，无需进行额外的信息交互。

附图说明

图1是本发明的基于单应性的移动机器人编队跟随方法的流程图；

图2是当前图像与期望图像之间单应性的计算示意图；

图3是跟随机器人局部坐标系的示意图；

图4是代表跟随机器人理想位姿的期望图像；

图5是跟随机器人拍摄到的实时图像；

图6是跟随机器人移动路径图；

图7是系统误差曲线图。

具体实施方式

下面根据附图和优选实施例详细描述本发明，本发明的目的和效果将变得更加明白，以下结合附图和实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

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

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

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

该步骤可以采用基于SIFT的特征点匹配的方式获取期望图像与初始图像两幅图中若干匹配点对的像素坐标，利用像素坐标计算两幅图之间的单应性矩阵，考虑到单应性的平面属性并且是为了描述跟随机器人位姿与领航机器人位姿的关系，故这些像素点对应位于领航机器人的且与相机光轴垂直的某一平面区域。

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

该步骤所用到的特征点跟踪是采用LK稀疏光流法实现的，首先寻找并记录特征点，在图像中选取特征较为明显的角点以及亚像素角点，然后基于LK光流法计算当前帧的特征点到下一帧的像素位置的移动即光流，最后通过对某些指标的判断筛选出特征点在下一帧图像中的正确位置，实现特征点在相邻两帧图像间的跟踪。根据该方法可快速计算出相邻两帧图像之间的单应性，鉴于单应性具备传递特性，任意非相邻两帧图像之间能够通过累乘的方式计算，因此进一步可快速计算出实时当前图像与期望图像之间的单应性矩阵，过程由图2表示：

图中H_it表示初始图像和期望图像之间的单应性矩阵，由步骤1得到，令下标k表示第k帧图像，相邻两帧单应性矩阵为H_k+1^k，称为增量式单应性，那么将当前图像与初始图像之间的一系列相邻帧的增量式单应性H_k+1^k累乘即可得到当前图像与初始图像之间的单应性H_ci，进而根据H_ct＝H_ciH_it得到当前图像与期望图像的单应性H_ct。

以跟随机器人当前位置为原点，朝向为z轴建立局部坐标系如图3所示，构造出一个虚拟机器人来表示跟随机器人理想位姿，在当前局部坐标系下坐标为(x_e,z_e,θ_e)，那么不难得出当前时刻跟随机器人与虚拟机器人之间的旋转矩阵和平移向量分别为：

跟随机器人位姿与虚拟机器人位姿即期望位姿之间的单应性矩阵可通过下列公式计算得到：

其中，设定属于领航机器人的且与相机光轴垂直的某一平面为参考平面，d^*表示期望位置到参考平面的距离即理想队形中跟随机器人与领航机器人的间距，为期望位姿坐标系下参考平面的法向量，实际情况中，虚拟机器人上的单目相机光轴也即期望位姿坐标系的z轴垂直于领航机器人的参考平面，也就是说，参考平面的法向量在期望位姿坐标系下的表示为那么单应性矩阵可进一步表示为：

上式表示的单应性矩阵H也即前文基于特征点跟踪所计算出的当前图像与期望图像的单应性H_ct，可以看出单应性矩阵中只有四个元素为变量，分别是h₁₁,h₁₃,h₃₁,h₃₃，利用这四个元素构造误差变量如下：

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

在世界坐标系下假设跟随机器人的坐标是(x_f,z_f,θ_f)，虚拟机器人的坐标是(x_v,z_v,θ_v)，领航机器人的坐标是(x_l,z_l,θ_l)，后两者的坐标满足：

假设跟随机器人和领航机器人的线速度及角速度分别为(v_f,ω_f),(v_ld,ω_ld)，在跟随机器人局部坐标系下，虚拟机器人与跟随机器人的相对位姿坐标为(x_e,z_e,θ_e)，该相对坐标与跟随、虚拟机器人的惯性坐标之间满足关系如下：

跟随机器人的运动学模型为虚拟机器人不满足一般的轮式机器人运动学模型，其模型为

取出单应性矩阵中非常量的元素h₁₁,h₁₃,h₃₁,h₃₃，根据公式(6)以及跟随、虚拟机器人的运动学模型估计出领航机器人的线速度和角速度，计算方法如下：

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

给定两个大于0的控制增益k₁,k₂，结合系统误差中的e₁,e₂以及估计出的领航机器人速度v_ld,ω_ld可计算出跟随机器人下一个运行周期的速度输入信号，计算方式如下：

k₁,k₂为经验值，通过实际实验确定合适的数值。

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

本发明针对以领航-跟随模型为代表的移动机器人编队系统，采用了一种基于单应性的编队跟随控制方案，在给定理想队形间隔距离和理想期望图像的前提下，利用单应性矩阵构造可反映理想队形中跟随机器人实时位姿的虚拟机器人，将原先的编队问题转化为对虚拟机器人的轨迹跟踪问题。编队跟随过程中，领航机器人的速度采用估计的方式，利用单应性与速度之间的关系模型以及跟随机器人的实时速度能较为准确的估计领航速度，从而避免采用局部通信的方式，节省了编队实验成本。本方法简单可行，能满足移动机器人编队跟随的要求。

给定如图4所示的期望图像代表理想队形中跟随机器人的位姿，跟随机器人轮间距为48cm，其几何中心固定单目相机采用罗技S5500网络摄像头，根据标定得到该摄像头的内参矩阵为

本实施例中的跟随机器人为履带机器人，领航机器人为简易模型，领航机器人从运动学角度上等价于一般的轮式机器人，由人对其提供动力，拉动其进行满足非完整约束的移动，控制增益为k₁＝0.045,k₂＝0.03，期望图像的距离为d^*＝0.35m。

图5,6,7表示了该方法的运行结果，图5和图6分别为跟随机器人在运行过程中所拍摄得到的实时图像以及实验者拍摄的机器人移动路线(部分截取图像)，实验中领航机器人在人为拖动下先直行然后左转最后直行到停止，从实时图像中可以看出跟随机器人在逐渐接近领航机器人(因为初始位姿比期望位姿远)，在转弯时跟随机器人与领航机器人保持一定的相对角度进而产生角速度信号以驱动其跟着转弯，最后直行到停止的部分以及最后的实时图像与期望图像非常相似，也说明了跟随机器人将与领航机器人保持理想队形实现编队跟随的任务。

实验中编队跟随系统的误差变化曲线如图7所示，最终误差收敛到(0.0061，-0.0042，-0.0078)，上面两幅是e₁,e₂的变化过程，尽管曲线波动较大，但依然从整体趋势上可以看出，位置误差e₁,e₂在控制律的作用逐渐收敛至接近0并有保持的趋势，方向偏差e₃也逐渐收敛到0。故从结果可以看出，本发明能够有效地实现常规的移动机器人编队跟随任务，控制效果稳定，收敛误差较小。

本领域普通技术人员可以理解，以上所述仅为发明的优选实例而已，并不用于限制发明，尽管参照前述实例对发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实例记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在发明的精神和原则之内，所做的修改、等同替换等均应包含在发明的保护范围之内。