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

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

A homography based method for mobile robot formation tracking

The invention discloses a homography-based formation following method for mobile robots. Given an ideal formation interval and an ideal desired image, a homography matrix is used to construct a virtual robot which can reflect the real-time pose of a random robot in an ideal formation, and the original formation problem is transformed into a pair of virtual machines. The trajectory tracking problem of robot. In the process of formation following, the speed of the navigation robot is estimated by using the model of the relationship between homography and speed, and the real-time speed of the random robot is used to estimate the navigation speed more accurately. This method is simple and feasible, and it can meet the requirements of mobile robot formation tracking.

【技术实现步骤摘要】
一种基于单应性的移动机器人编队跟随方法
本专利技术涉及一种轨迹跟踪方法，尤其涉及一种基于单应性的移动机器人编队跟随方法。
技术介绍
移动机器人编队控制是一类典型的多机器人协作问题，在军事、生活、工业等方面具有广泛的应用前景，这类问题研究主要集中于调度、跟踪等智能交通领域，近几十年来一直是多机器人协作相关研究的热点和难点，该问题的研究对于多机器人需要共同完成作业时起到重要的作用。编队跟随本质上属于轨迹跟踪问题，因此目前大多数编队控制的研究是基于轨迹跟踪研究的基础上进行的，传统的研究方法主要有：将原先非线性系统分解为多个低阶子系统，对子系统设计部分李雅普诺夫函数进而设计跟踪控制器，或者通过将系统转换为串联或链式系统形式，利用反步法设计轨迹跟踪控制器等。这些传统方式均是在机器人位姿信息已知的前提下进行的，实际场景中需要考虑机器人位姿信息的获取，通常采用激光、雷达等距离传感器或者建立局部通信网络等方式实现。近些年基于单目相机的机器人视觉伺服控制快速发展，使得一些相关研究逐渐围绕单目视觉反馈来实现，常见的方法有：利用单目相机拍摄一系列周边图像，再辅以激光传感器获取的深度信息，构造出局部三维地图实时定位机器人位姿信息，进而完成编队任务，其成本较为高昂且控制器设计难度高；通过附加标记点来估计相关位姿，前提是需要事先标记且记录相关信息，提供较多先验知识；基于多视图几何的方法更为广泛，如极点和单应性分解，但是同样存在诸如极点奇异或分解不唯一等影响系统稳定性的问题。
技术实现思路
本专利技术针对现有技术的不足，提供一种基于单应性的移动机器人编队跟随方法，该方法的具体技术方案如下：一种基于单应性的移动机器人编队跟随方法，用于移动机器人在多机器人协作系统中的编队跟随控制，该方法采用领航-跟随模型，所述的领航机器人和跟随机器人均为轮式机器人，且跟随机器人配置单目相机可实时采集图像信息，其特征在于，包括如下步骤：步骤1：在初始时刻，给定表示理想队形中跟随机器人位姿的期望图像和距离信息，利用跟随机器人上安装的单目相机获取当前图像，作为初始图像，根据期望图像与初始图像计算单应性矩阵并记录；步骤2：在步骤1中得到初始、期望两幅图之间的单应性矩阵后，在初始图像属于领航机器人的且与相机光轴垂直的某一平面区域中选取若干特征点；步骤3：给定信号让跟随机器人开始运行，运行过程中不断对这些特征点进行跟踪以此来计算当前图像与期望图像之间的单应性矩阵，进一步依据单应性矩阵元素构造系统误差；步骤4：根据步骤3中计算得到的当前图像与期望图像的单应性矩阵计算并记录本次运行周期中跟随机器人的线速度和角速度，当运行过一个周期计算出下一个当前图像与期望图像之间单应性矩阵；结合上一周期和当前周期的单应性矩阵以及上一周期的跟随机器人的线速度和角速度估计出领航机器人的线速度和角速度；步骤5：将步骤3得到的实时系统误差和步骤4估计出的领航机器人的线速度和角速度共同作为控制器的输入信号，再由控制器输出信号驱动跟随机器人运动，运行过程中领航机器人始终自主运动；步骤6：跟随机器人接收驱动信号进行移动，同时利用单目相机获取实时当前图像，重复进行步骤3、4、5，经过这些步骤后跟随机器人将逐渐与领航机器人形成期望图像所指定的理想队列。优选地，步骤3中单应性矩阵的计算公式如下：其中(xe,ze,θe)表示跟随机器人与其期望位置的相对位姿，d*表示期望位置到所述的平面区域的距离即理想队形中跟随机器人与领航机器人的间距。优选地，步骤3中所构造的系统误差由如下公式计算：其中h11,h13,h31,h33为单应性矩阵中的元素。优选地，步骤4中可根据如下公式来估计运行过程中领航机器人的移动线速度和角速度：其中(vf,ωf)为当前运行周期跟随机器人的线速度与角速度。优选地，步骤5中控制器输出用于驱动跟随机器人的速度信号由如下公式计算：其中k1,k2是控制增益，根据实际实验取值。本专利技术的有益效果：本专利技术利用基于特征点跟踪的方式结合单应性传递特性来获取系统误差，提高了系统运行的实时性，利用单应性与速度之间的关系模型以及跟随机器人的实时速度较为准确地估计领航机器人速度，无需进行额外的信息交互。附图说明图1是本专利技术的基于单应性的移动机器人编队跟随方法的流程图；图2是当前图像与期望图像之间单应性的计算示意图；图3是跟随机器人局部坐标系的示意图；图4是代表跟随机器人理想位姿的期望图像；图5是跟随机器人拍摄到的实时图像；图6是跟随机器人移动路径图；图7是系统误差曲线图。具体实施方式下面根据附图和优选实施例详细描述本专利技术，本专利技术的目的和效果将变得更加明白，以下结合附图和实施例，对本专利技术进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本专利技术，并不用于限定本专利技术。如图1所示，一种基于单应性的移动机器人编队跟随方法，用于移动机器人在多机器人协作系统中的编队跟随控制，采用领航-跟随模型，所述领航机器人和跟随机器人均为轮式机器人，且跟随机器人配置单目相机可实时采集图像信息，该方法包括如下步骤：步骤1：在初始时刻，给定表示理想队形中跟随机器人位姿的期望图像和距离信息，利用跟随机器人上安装的单目相机获取当前图像，作为初始图像，根据期望图像与当前初始图像计算单应性矩阵并记录；步骤2：在步骤1中得到初始、期望两幅图之间的单应性矩阵后，在初始图像属于领航机器人的且与相机光轴垂直的某一平面区域中选取若干特征点；该步骤可以采用基于SIFT的特征点匹配的方式获取期望图像与初始图像两幅图中若干匹配点对的像素坐标，利用像素坐标计算两幅图之间的单应性矩阵，考虑到单应性的平面属性并且是为了描述跟随机器人位姿与领航机器人位姿的关系，故这些像素点对应位于领航机器人的且与相机光轴垂直的某一平面区域。步骤3：给定信号让跟随机器人开始运行，运行过程中不断对这些特征点进行跟踪以此来计算当前图像与期望图像之间的单应性矩阵，进一步依据单应性矩阵元素构造系统误差；该步骤所用到的特征点跟踪是采用LK稀疏光流法实现的，首先寻找并记录特征点，在图像中选取特征较为明显的角点以及亚像素角点，然后基于LK光流法计算当前帧的特征点到下一帧的像素位置的移动即光流，最后通过对某些指标的判断筛选出特征点在下一帧图像中的正确位置，实现特征点在相邻两帧图像间的跟踪。根据该方法可快速计算出相邻两帧图像之间的单应性，鉴于单应性具备传递特性，任意非相邻两帧图像之间能够通过累乘的方式计算，因此进一步可快速计算出实时当前图像与期望图像之间的单应性矩阵，过程由图2表示：图中Hit表示初始图像和期望图像之间的单应性矩阵，由步骤1得到，令下标k表示第k帧图像，相邻两帧单应性矩阵为Hk+1k，称为增量式单应性，那么将当前图像与初始图像之间的一系列相邻帧的增量式单应性Hk+1k累乘即可得到当前图像与初始图像之间的单应性Hci，进而根据Hct＝HciHit得到当前图像与期望图像的单应性Hct。以跟随机器人当前位置为原点，朝向为z轴建立局部坐标系如图3所示，构造出一个虚拟机器人来表示跟随机器人理想位姿，在当前局部坐标系下坐标为(xe,ze,θe)，那么不难得出当前时刻跟随机器人与虚拟机器人之间的旋转矩阵和平移向量分别为：跟随机器人位姿与虚拟机器人位姿即期望位姿之间的单应性矩阵可通过下列公式计本文档来自技高网...

【技术保护点】
1.一种基于单应性的移动机器人编队跟随方法，用于移动机器人在多机器人协作系统中的编队跟随控制，该方法采用领航‑跟随模型，所述的领航机器人和跟随机器人均为轮式机器人，且跟随机器人配置单目相机可实时采集图像信息，其特征在于，包括如下步骤：步骤1：在初始时刻，给定表示理想队形中跟随机器人位姿的期望图像和距离信息，利用跟随机器人上安装的单目相机获取当前图像，作为初始图像，根据期望图像与初始图像计算单应性矩阵并记录；步骤2：在步骤1中得到初始、期望两幅图之间的单应性矩阵后，在初始图像属于领航机器人的且与相机光轴垂直的某一平面区域中选取若干特征点。步骤3：给定信号让跟随机器人开始运行，运行过程中不断对这些特征点进行跟踪以此来计算当前图像与期望图像之间的单应性矩阵，进一步依据单应性矩阵元素构造系统误差；步骤4：根据步骤3中计算得到的当前图像与期望图像的单应性矩阵计算并记录本次运行周期中跟随机器人的线速度和角速度，当运行过一个周期计算出下一个当前图像与期望图像之间单应性矩阵；结合上一周期和当前周期的单应性矩阵以及上一周期的跟随机器人的线速度和角速度估计出领航机器人的线速度和角速度；步骤5：将步骤3得到的实时系统误差和步骤4估计出的领航机器人的线速度和角速度共同作为控制器的输入信号，再由控制器输出信号驱动跟随机器人运动，运行过程中领航机器人始终自主运动；步骤6：跟随机器人接收驱动信号进行移动，同时利用单目相机获取实时当前图像，重复进行步骤3、4、5，经过这些步骤后跟随机器人将逐渐与领航机器人形成期望图像所指定的理想队列。...

【技术特征摘要】
1.一种基于单应性的移动机器人编队跟随方法，用于移动机器人在多机器人协作系统中的编队跟随控制，该方法采用领航-跟随模型，所述的领航机器人和跟随机器人均为轮式机器人，且跟随机器人配置单目相机可实时采集图像信息，其特征在于，包括如下步骤：步骤1：在初始时刻，给定表示理想队形中跟随机器人位姿的期望图像和距离信息，利用跟随机器人上安装的单目相机获取当前图像，作为初始图像，根据期望图像与初始图像计算单应性矩阵并记录；步骤2：在步骤1中得到初始、期望两幅图之间的单应性矩阵后，在初始图像属于领航机器人的且与相机光轴垂直的某一平面区域中选取若干特征点。步骤3：给定信号让跟随机器人开始运行，运行过程中不断对这些特征点进行跟踪以此来计算当前图像与期望图像之间的单应性矩阵，进一步依据单应性矩阵元素构造系统误差；步骤4：根据步骤3中计算得到的当前图像与期望图像的单应性矩阵计算并记录本次运行周期中跟随机器人的线速度和角速度，当运行过一个周期计算出下一个当前图像与期望图像之间单应性矩阵；结合上一周期和当前周期的单应性矩阵以及上一周期的跟随机器人的线速度和角速度估计出领航机器人的线速度和角速度；步骤5：将步骤3得到...

【专利技术属性】
技术研发人员：刘山，曹雨，
申请(专利权)人：浙江大学，
类型：发明
国别省市：浙江,33